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detection systems were designed and implemented in a Simulink model of the “21UUV.” In 
the course of conducting simulations with the model, numerous vehicle behaviors were 
studied and detection response was verified. In addition, the model based observer residuals 
may be designed to distinguish actuator faults from wave disturbances and fin faults from 


maneuvering responses. 
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I. INTRODUCTION 


A. GENERAL BACKGROUND AND LITERATURE 

The increased desire to use Autonomous Underwater Vehicles (AUVs) and 
Unmanned Undersea Vehicles (UUVs) for commercial and military applications has led 
to a great deal of research in this field over the last decade. The military, as well as 
industry, can see the great potential uses for AUVs and UUVs in the oceanic 
environment. Although great strides have been made in this field of research, to develop 
both methodology and technology pieces, further work is needed in precision navigation, 
sensor development and integration, and especially improving the reliability of long term 
mission completion. 

Previous work on AUV technology shows that underwater navigation to sufficient 
precision within cost limits is possible. AUV uses for oceanographic survey has been 
described in Belneaain (1997) have given results on positioning accuracy for survey 
missions. Marco and Healey (1996) have demonstrated a method to navigate an AUV in 
a local area using an acoustic sensor for position information derived from feature 
detection. Healey and Lienard (1993) have shown that multivariable sliding mode 
autopilot based on state feedback, designed assuming decoupled modeling, is quite 
satisfactory for the combined diving, steering, and speed response of a slow speed AUV. 
Cristi, Healey, and Papoulias (1990) have illustrated that adaptive sliding mode contro! in 
the dive plane is possible for AUVs. Now that cost effective, precise underwater 
navigation is becoming possible, the remaining major technical issue is the overall 


reliability of the vehicle for long term and complex missions. 


In order for AUVs and UUVs to be a more robust, self-sufficient system, an on- 
line failure detection and resolution system is needed. The failure detection and 
resolution system must work in tandem with an AUV or UUV whose systems are 
reconfigurable, so that minor faults may be overcome by control reconfiguration. This 
area of study, commonly linked to failure modes and effects analysis (FMEA), has 
received a great deal of vale the past several years. Previous literature pertinent to 
this research is abundant since the aircraft, spacecraft, and process industries have all 
written about fault detection techniques. A brief summary of some basic fault detection 
techniques along with some examples can be found in Gertler (1986). Bell, et al., (1992) 
has developed a tool that automates the reasoning portion of an FMEA. The prototype 
has been created and successfully passed a test and evaluation program. Healey (1992) 
has addressed the use of Kalman Filters and Artificial Neural Networks to provide 
detection, and isolation of impending subsystem failures. Finally, Hurni (1997) shows 
that Simulink can be used as a modeling and simulation tool for FMEA on an AUV 
steering system. In real time control of autonomous systems, however, failure detection 
and accommodation is required as part of an overall system controller. 

This study concentrates in a specific area of fault detection analysis; the use of 
model based observers for fault detection. Previous literature in this particular area 1s 
also abundant. 

Early approaches to analytical redundancy for fault detection have been described 
in the surveys by Wilsky (1976) and Isermann (1984), more recently, Patton (1997). The 
system model includes models of the anticipated faults - often as an additive input or 


Output, or as a multiplicative factor as in a parameter change. Use of a bank of Kalman 


Filters, each “tuned” to a particular fault are then used to generate a “residual” — the 
innovation (see Napolitano and Swain (1989) for an aircraft application). The statistical 
properties of the residual are then tested against either single or multiple hypotheses and 
the residual with the maximum likelihood compared to a threshold is selected as the 
identifier of a fault. If no faults occur, all residuals are zero. 

Problems with the above approaches are that the model based filter residuals are 
sensitive to both faults and disturbances. In fact, they are also sensitive to unmodeled 
dynamics and coupling inputs from other response modes in the system. Also, while the 
observer is less sensitive to input commands than a servo error detector, there are 
maneuvering responses that occur even when faults are absent. 

Eigenstructure analysis, Speyer (1987) and Patton and Chen (1991), solves the 
problem of eigenvector as well as eigenvalue design in the residual generation system 
such that isolatable disturbance responses can be distinguished from fault responses, and 
a weighted residual measure can lead to either left or mght eigenvector consideration. 
Alternatively, the fault model is embedded into the filter and the fault state is identified as 
a state output from the filter - its mean, together with its covariance, so that again 
likelihood measures can be assessed. This approach is described in Mangoubi et. al., 
(1995) with specific application to an underwater vehicle, who proposed a “robust” 
likelihood measure for separating unmodelled dynamics from an actuator fault. 

Still, the problem of robustness especially to intermode coupling, simplicity of 
software management, and threshold design, is problematic when it is considered that 
future UUV missions will require long term reliability for shallow water operations in 


energetic (high sea states) environments. Healey (1998) has illustrated a proposed fault 


detection architecture that weights inputs from servo error detectors, wave action 
detectors, residual generators, into a fuzzy inference systems that is conjectured to 
provide robustness and simplicity through decomposition. This thesis and related work is 


a first step, aimed at demonstrating the correctness of that assertion. 


B. SCOPE OF THIS WORK 

The overall problem of autonomous fault detection is complex and diverse. This 
study will focus on sensor based fault detection limited to the pnmary subsystems of the 
Naval Undersea Warfare Center’s experimental UUV (21UUV), including the diving, 
steering, roll and speed control systems. In particular, model based observer residuals for 
the detection of fin faults and weight buoyancy mismatches in shallow water operations 
with wave effects is discussed. The purpose of this thesis is threefold: 


1. To design model based observers for the diving, steering, and roll control 
systems of the 21UUV; 


2. To implement the model based observers into a computer model of the 
21UUV; and 


3. To run simulations on the 21 UUV computer model to determine if model 

based observers can robustly detect fin faults and weight-buoyancy mismatches in 

a shallow water environment. 

Chapter II discusses different types of faults that can occur on UUVs. Most faults 
can be distinguished as hardware/software problems on the vehicle, although some 
environmental disturbances can be defined as a “fault” since they jeopardize mission 
completion. In addition, advantages and disadvantages of different types of fault 


detection techniques are described including limits and trends analysis, model free 


detection, and model based detection. 


Chapter III describes a proposed fault detection architecture of 21UUV and 
discusses the design of model based observers for the diving, steering and roll control 
systems. The design of each model based observers is divided into a “theory” and a 
“application” section. The application section shows how each observer was 
implemented into the 21UUV computer model. 

Chapter IV is intended to be a “User’s Guide” for the Simulink 21UUV computer 
model. A brief description of each section is given and appropriate Matlab files are 
referenced. A detailed description on how to input data into the program for successful 
simulations is also included. 

Chapter V shows results that prove model based observer residuals can be used 
for the detection of fin faults and weight-buoyancy mismatches in the shallow water 
environment. Simulations were conducted on the 21UUV model to verify that fin faults 
could be distinguished from maneuvering responses, and that fin faults could be 
distinguished from wave disturbances. 

Chapter VI lists the conclusions of this report derived for the results of Chapter V. 


In addition, recommendations are made for additional study. 
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Wl. FAULT TYPES AND DETECTION / DIAGNOSTIC TECHNIQUES 


Long term deployments of autonomous systems in the ocean require 
replenishment of energy supplies and reliable fault free operation. It is recognized that 
fault free operation will not always be possible, so that system design must pay attention 
to a study of failure modes and effects. The purpose of this chapter is to describe 


different types of faults and possible detection methods common to the UUV. 


A. FAULT TYPES 
In spite of good engineering practice, faults can occur. Two kinds of faults can be 
identified: 


1. Those that arise from malfunctions in the hardware and software subsystems in 
the vehicle; and 


2. Those that arise from environmental conditions that are viewed as. 
disturbances, and while these may not be direct malfunctions, they have the effect 
of performance degradation and the completion of a mission is jeopardized. 

Even though UUVs are usually equipped with highly reliable sensor suites, 
connections and computing components, hardware and software faults can occur in the 
vehicle. An example of a hardware fault would be the loss of steering resulting from a 
stuck or loose fin. 

Since the UUVs sensors and computing components are normally highly reliable 
and accurate, environmental conditions are easily detected by the vehicle. These 
disturbances may cause significant variations in inertial velocities as well as translational 


and rotational measurements. An example of an environmental condition “fault” would 


be the inability of the vehicle to take a data measurement because of a high sea state in 
shallow water operations. 

Another way to classify faults is by time. Incipient faults occur when a signal or 
system gradually degrades over a long period of time. Incipient faults are difficult to 
detect because the signal varies very slowly over time. 

Abrupt faults are those that usually cause a “jump” variation in the signal in a 
short period of time. These are the types of signals that can be more easily detected to 
quickly determine actuator, sensory and plant faults on UUVs. Examples include a 
sudden loss of power to a sensor, or a sudden loss of a mechanical linkage (propeller 


breaks free). 


B. DETECTION / DIAGONOSTIC TECHNIQUES 

Many papers have been written on how to design a system that will automatically 
detect the presence of a fault. Research is not limited to just UUVs. The aircraft, 
spacecraft, and process industries have also written about fault detection techniques. As 
described by Gertler (1986), fault detection methods can be classified into three methods: 
those that use simple limits and trends analysis, those that use detection techniques which 
are without the use of analytical models, and those that provide models as the basis for 


detection filters. 


1. Limits and Trends Analysis 
A survey of fault detection methods indicates that alarms can be easily monitored 


if signals are static. This is done using “limits and trends” analysis. In this method, the 


measured signal is compared to a previously set threshold. Exceeding the threshold 
would indicate a fault condition that would be passed to a higher level controller for 
subsequent action. Limits and trends analysis is suitable for static or slow varying signals 


such as computer bay temperatures and battery voltage, Fossen (1994). 


De Model Free Detection 

Limits and trends analysis is not suitable for the detection of dynamic signals such 
as wave action. Dynamic signals tend to exceed threshold limits, only to come back 
within bounds a short time later. This causes thresholding alone to be a problem. 

Model free methods attempt to extract a constant feature of a signal to compare 
against a threshold value. This is the case when a spectral analysis is performed on a 
signal and spectral levels in specified frequency bands can be compared against 
thresholds. The model free method is useful to detect the presence of frequency 
components in servo error signals and could be used to identify levels of wave induced 


disturbances considered as faults, Newland (1993). 


3. Model Based Residual Generation 

Model free methods have difficulty detecting dynamic signals developed from 
autopilot errors. These error signals are naturally large when steering to new commands, 
but small if correct final heading is maintained. In addition, wave motion causes wave 
period oscillatory motion in servo errors. 

Model based methods have been found to be useful over the last 20 years or so in 


detecting dynamic signals embedded in noise. By using a model based observer, a 


“residual” can be generated between the sensor measured values and that predicted for 
the model. The residual provides a signal that is not sensitive to servo errors caused by 
command changes, and responds primarily to non ideal loads, disturbances from waves, 
and sensor signal errors. Likelihood functions are then used to provide probability ratios 
corresponding to fault based hypotheses. Simple or multiple hypothesis testing is done 
with decision making on the basis of threshold exceedance. The List of References gives 
a listing of papers relevant to the methods, Isermann (1984), Patton (1997), Speyer 


(1987), Willsky (1976), Beard (1971) and Jones (1973). 


it, FAULT DETECTION ARCHITECTURE AND OBSERVER DESIGN 


A, PROPOSED FAULT DETECTION ARCHITECTURE FOR 21UUV 

The overall problem of autonomous fault detection for 21UUV and similar Navy 
vehicles is complex. Therefore, the focus of sensor based fault detection will be limited 
to the primary subsystems of the vehicle, including the diving, steering, speed control, 
roll control, navigation, powering, and computer subsystems (sensor outputs from the 
vehicle as opposed to environmental sensors such as forward looking or side scan sonar). 
As far as motion capabilities are concerned, the diving, steering, speed and roll control 
subsystems should each be monitored. Faults that impair the capability of these 
subsystems need to be detected so that reconfiguration of control settings may help to 
mitigate a premature mission abort. Some graceful degradation of mission performance 
could be allowed if “partial” depth control or speed control could be maintained. 

The proposed fault detection architecture for 21UUV consists of sensor outputs 
from the vehicle, controllers, fault detectors and a fuzzy inference system. Sensor 
outputs from the vehicle feed into the controllers and the fault detectors. The controllers 
provide control inputs back into the vehicle and to the set of fault detectors. The fault 
detectors compare inputs from the sensor outputs and the controller and use residual 
generators to determine if a fault has occurred. If a fault has occurred, the signal is 
passed to the fuzzy inference system to determine the accommodating response. The 
fuzzy inference system is aimed at providing robustness and determines whether the fault 
can be handled by making a command adjustment to the controllers or a health 


assessment to a higher level control is needed. Because residual generation is known to 


i] 


be sensitive to unmodelled inputs (coupling from other modes of response and 
disturbances, as well as faults, the weighting of inputs from several observation systems 
is expected to increase the reliability of fault detection. Figure 3.1 shows how this is 


illustrated. 
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Figure 3.1 Proposed Fault Detection Architecture for 21 UUV (Healey, 1998) 


Four different sets of fault detectors are used in this architecture to improve 
robustness: fin stroke detectors, servo error detectors, observer residual detectors, and 
wave activity detectors. The fin stroke and servo error detectors use simple detection 
circuits that look for signal magnitude as well as length of time persisting in the fault 
mode that produces the fault declaration. These are model free detectors. An observer 
residual detector 1s a model-based detector that is used to generate a “residual” between 
the sensor measured values and that predicted from the model using the same control 


input as applied to the vehicle. The residual magnitude 1s processed and compared to a 


I 


threshold value to determine exceedance. Then, the residual is processed through a non- 
linear filter, with a forgetting factor, to determine the persistence of the fault. Finally, the 
level of severity of the fault is determined through quantization. An example of this 


exceedance and persistence scheme is shown in Figure 3.2. 
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Figure 3.2 Exceedance and Persistence Detection (Simulink Model, Healey, 1998) 


The fault detection architecture for 21UUV must also take into account operation 
near the surface under waves. By comparison of residual signals, a distinction can be 
made between wave disturbance and an actuator fault condition. Robustness 1s improved 
when a wave activity detector 1s added. The combination of all sources of information 


allows for the accommodation response using a fuzzy inference system linking residual 
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fault declaration signals as inputs that are mapped to recommended actions using fuzzy 


rules. 


B. DESIGN OF MODEL BASED RESIDUAL OBSERVERS 
Dynamic signals such as those developed by autopilot errors become difficult to 
detect, as errors are naturally large when steering to new commands, yet are small if 
correct final heading is maintained. Wave motion causes wave period oscillatory motion 
in servo errors. As described in earlier chapters, many methods are available for fault 
detection, but model based methods, like observer residual detectors, have been found to 
be useful in detecting dynamic faults and are the subject of this work. Faults in this 
context, can arise from a bad sensor as well as a faulty actuator (fin, propeller), and may 
be modeled as an added force or sensor output - generally of unknown magnitude - the 
presence of which may. be detected by residual analysis. The residual provides a signal 
that is less sensitive to servo errors caused by command changes, and responds primarily 
to non ideal loads, disturbances from waves, and sensor signal errors. Analysis of the 
residuals provides the key to subsystem failure detection. The design of the 21UUV 
diving, steering, and roll control observer reside detectors will be discussed, using the 
method outlined in Healey (1998) and Patton and Chen (1991). 
The assumption on which our approach to fault detection is based is that a UUV, with 
six degrees of freedom, may be controlled by four main subsystems that — in the ideal 
case — are uncoupled. Coupling is known to exist, but is ignored in the approach and 


assumed to be negligible for the purposes of control system design. 


The assumption is that there are four autopilot controllers — the steering, diving, roll 
and speed control systems, respectively. Because of that assumption, it follows that there 
should be four observer based residual generators — one for each controller — that would 
generate, and process residuals for each subsystem. 

Each subsystem is modeled as a non interacting LTI system: 
S:(A,B,C,D)e R™ 
where it is allowed that the parameters could be speed dependent and either robust 


control design methods are used, or the autopilots could be ‘gain scheduled.’ 
1. Diving Observer Residuals Detector 


a. Theory 
The diving subsystem dynamics for the 21UUV are modeled by the 
following equations: 


x'() = [0 ,(), G0, 91), Z(Y]; ult) = 5,() with 
X() = Ax (t) + Bu(t) + Ef ,() + Fd (i); 


yy =-Cx(0 +f; 


wr, 9, 8 and Z are the heave velocity of the vehicle relative to the water, 
the pitch rate, the pitch angle, and the depth. B and E are input vectors for the control 
planes and added forces and moments caused by the imbalance of commanded and actual 
loads on the vehicle caused by actuator faults. F is the input vector associated with 
disturbances from waves and currents. It is important to note that since the relative 


velocity definition for vehicle states is used, E and F are distinct (i.e. E F = 0) with the 
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result that disturbances from waves and currents are distinguishable from actuator faults 
[Patton and Chen (1991)]. With a high quality inertial system, all state variables are 
measured with little noise. Thus, the output matrix, C, may be considered to be identity. 
f,(t) and f;(t) are considered to be added forces caused by fin faults, and sensor errors 
respectively. 


From the subsystem dynamics, a model based observer can be formed: 


i) = [¥,(), 4), 0), ZW]; ult) = 6, () with 
x(t) = (A — KC )i(t) + Bu(t) + Ky (0); 
vi) = yi) - Ci); 


The residuals, v(t), are the differences between the sensor measured states 


and the model based estimates from the observer. It follows that, the state observation 


error, &, 1s given by: 


é,() = (A — KC Je, () + Ef ,() + Fd () + Kf); 
vi) = Ce, +t; 


The residuals, v(t), can be expressed as the sum of effects arising from the 
natural dynamics of the observation system plus responses to actuator or other additive 
forces embodied in f,(¢) , disturbances d(#), and sensor signal faults, f;(7). 

If C =I, then a residual is produced for each state and , if disturbances 
and faults are time invariant, the steady state (no transient) response of state residuals is 
given by: 


(00) = AES (0) + Az'Fd(o ) + Ax'Kf,(o0) 
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Now, ignoring sensor faults in this work, the following is observed: 


1. The influence of f, in a residual v, = C;vis nulled if C;A,‘E = 0. A,” is 
diagonal and C,E = 0 (i.e. C; lies in null[(A. E) J); 


2. The influence of d is a residual v, = C;v and is nulled if C; A, 'F = 0. 


The design key is to select K, such that A, has real eigenvectors. A, is 
diagonal and C; is chosen to lie in the null space of [A, K']’ to be orthogonal to E to 
suppress disturbances and to F for actuator faults, and to K to suppress sensor faults. 
Unlike state observation, residual generation requires a different balance in the choice of 
filter gains, Ky. Too high of a choice leads to the residual being small, dominated by 
sensor faults and noise. A lower gain set is needed consistent with bandwidth 
requirements. Of course, stability must be obtained. 

The residuals can be analyzed in the frequency domain by: 


v(s) = C(sI- A.) 'Ef (5s) + C(G1- A.) 'Fd (s) + C(S1- A.) 'Kf (5); | 
where A, =(A-— KC ) 


One observer design (low gain) should therefore find a gain set that 
maximizes the influence of actuator faults, minimizes the influence of wave disturbance, 


and the sensor faults. 


b. Application 
Using the 21UUV model (described in chapter four) running at a forward 
speed of 6 ft/sec, x =[w,, g, @ Z]. For a slow forward speed of 6 feet per second, the 


dynamics and input matrices are: 
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-~0.1140 2.3282 -0.0019 0 ~ 0.3308 

| 0.0649 -0.3015 0.0109 0| — ~0.1224 

0 l 0 Ol” ae | ang 
] 0 -6 0 0 


In this example, the vehicle is moving forward with a command to dive. 
Wave amplitude is set at 2 ft. The autopilot for depth control is a sliding mode design. 
The placed poles are selected to include a single pole at the origin (required by method), 
yielding: 
A=|[-04 -0.41 -0.42 0] 


k =|-0.5100 -5.2770 -2.0321 0] 
s'=[0.0663 -0.7046 -0.7062 0.0205] 


and the control law (Healey, Lienard, 93): 


6,(t)=-ke+7 sats © 
9? 


Several different options can be used for the selection of the observer gain, 
K,. One option is to use a linear quadratic estimator. This method leads to undesired 
complex poles and eigenvectors. A better solution is to use real pole placement. This 
guarantees real poles and eigenvectors. The observer gains are found using the Matlab 
‘place’ algorithm to put the observer poles at real values close to the control poles, [-0.2, 


-0.21, -0.22, -0.24]. The solution for K, yields: 
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0.0860 2.3282 -—0.0019 0 

_ {0.0649 -0.0915 0.0109 0 

: 0 l 0.2200 0 
I 0 ae 0.2400 


The observer is modeled by the state-space equation: 


x'(t) = A,x()+B,u() 
y(t) = C,x() + u(t) 
where : 

A, =A-K,'*C 
B,=[B K,'] 
C,=-C 


De = [zeros (41) eye(4,4)] 


For the diving observer residuals detector: 


-0.20 0 0 0 

0 -021 0 0 
Noe 

0 0 -0.22 0 

0 0 0 -0.24 


—0.3308 0.0860 2.3282 -0.0019 0 
-—0.1224 0.0649 -0.0915 0.0109 0 


: 0 0 ] 0.2200 0) 
0) 1 0 —6 0.2400 
~ Ome a0 01000 
0 -1 0 O 00 1 0 0 
C= ; D,= 
0 oO -1 0 00010 
Qe ty 0000 1 


The Matlab file used to design the observer, “dive_obs des”, is included 


in Appendix A. 


2. Steering Observer Residuals Detector 


a. Theory 
The steering subsystem dynamics for the 21UUV may be modeled by the 
following equations: 


x'() = [v, (0, F7), WY]; ul) = 6,() with 
x(t) = Ax (t) + Bu(t) + Ef ,@) + Fd (); 


yy =Cx(p+f,0; 


v,, r, and ware the sway velocity of the vehicle relative to the water, the 
yaw rate, and the yaw angle. Besides the different variables listed above, the steering 


observer is designed in the same matter as the diving observer. 


b. Application 


Using the 21UUV model (described in chapter four) running at a forward 
speed of 6 ft/sec, x =[v,, 7, and y]. For a slow forward speed of 6 feet per second, the 


dynamics and input matrices are: 


—0.1140 -—2.3282 0 0.3308 
A =;-—0.0649 -03015 0] ; B=;}-0.1224 
0 l 0 0 


In this example, the vehicle is moving forward with a command to steer. 


Wave amplitude is set at 2 ft. The autopilot for depth control is a sliding mode design. 
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The placed poles are selected to include a single pole at the origin (required by method), 


yielding: 


A=|-04 -041 of 
k =10.5762 -1.6663 0| 
s'=[0.0164 0.8804 0.4740] 


The observer gains are found using the Matlab ‘place’ algorithm to put the 
observer poles at real values close to the control poles, [-0.2, -0.21, -0.22]. The solution 


for K, yields: 


0.0860 -23282 0 
K, =|-0.0649 -0.0915 0 
0 1 0.2200 


The observer is modeled by the state-space equation: 


x'() = A,x() + Buy 
y(t) =C,x()+ Duy) 


where : 


A, =A-K, *C 


B, B K, | 
—C 
[zeros (3,1) eye(3,3)| 


C, 
D, 


For the steering observer residuals detector: 
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— 012006 0 0 


A,=| 0 -02100 0 

0 0  -0,2200 

0.3308 0.0860 -2.3282 0 
B, =|-0.1224 -0.0649 -0.0915 0 
0 0 1 0.2200 

lwo 0 0100 
C=) Oi Ol aD OmOml 0 
One.0, —| Common | 


The Matlab file used to design the observer, ‘steer_obs_des’, is included 


in Appendix A. 


3. Roll Observer Residuals Detector 


a. Theory 
The roll subsystem dynamics for the 21UUV may be modeled by the 


following equations: 


x') = [6(), BID]; ut) = 6, with 
i() = Ax(t) + Bu(t) + Ef ,() + Fd): 
y(t) = Cx) + £,(); 


gand p are the roll angle and roll rate. Besides the different variables 


listed above, the roll observer is designed in the same matter as the diving observer. 


ys 


b. Application 


Using the 21UUV model (described in chapter four) running at a forward 
speed of 6 ft/sec, x =[¢ and p]. For a slow forward speed of 6 feet per second, the 


dynamics and input matrices are: 


0 ] 0 
A — ; B = 
Fi 1.7474 i 


In this example, the vehicle is moving forward with a command to steer. 
Wave amplitude is set at 2 ft. The autopilot for depth control is a sliding mode design. 


The placed poles are selected to include a single pole at the omgin (required by method), 


yielding: 


A=[-14 -1.4)] 
—k=([+1.2308 0.9861] 
s'=[0.8137 0.5812] 


The observer gains are found using the Matlab “place” algorithm to put 


the observer poles at real values close to the control poles, [-1.4, -1.41]. The solution for 


K, yields: 


oe 1.4000 —1.7474 
| 1.4100 


The roll observer is sensitive to centrifugal force action during steering 


maneuvers. Thus, a feed forward control system, Gg, was added to B,: 
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where : 

m = 88.95 slugs 
k, = (02 

U = 6ft/sec 
T= 3298 


mis the mass of the vehicle, kis a gain constant, U is the design velocity of the observer, 
and J, is a dimensionless coefficient. The observer is modeled by the state-space 
equation: 


x") = A,x())+ Buy 
y() =C,x()+ Duly 


where : 

A. =A-K, *C 

B, -|B K, 0,6,) | 
@ SxE 


D, = [zeros(2,1) eye(2,2) zeros(2,1)| 


For the roll observer residuals detector: 


Ne -140 0 7 0 1.4000 ] 0 
= | 0 S14 ee ee Blomeise— 17474 194100 0.5387 


-1 0 oF 00 
C — a D, = 
QO -] 00 1 0 
The Matlab file used to design the observer, “roll_obs_des”, is included 
in Appendix A. 
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VI. 21UUV COMPUTER MODEL AND SIMULATION 


A. MODEL OVERVIEW 

This chapter describes the 21UUV model and how inputs were made to produce 
desired simulations. Modifications were made to the original model designed by Healey 
and Miguel to accommodate specific simulations. The model is a combination of 
Simulink and Matlab files. Matlab files used in the model are in Appendix B. Figure 4.1 
shows an overview of the 21UUV computer model. 


To Workspace To Worktpacel 


Workspace 


Inertial Posthon and 
euler angles 





| f 
Speed (ucom) 
Command 
exignals 
Pies | Pa niene 
To Workspace? 
ee To weap 
i | To Workspace! ! 

Ss 
ec 
maaeal wectaet | TeWeccpocel 
Ware_Mction _Detection To Workspacel4 


Figure 4.1 21UUV Computer Model 
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Simulations were run using the Simulink file “sim_uuv.” The command 
“start_up” runs an Matlab file that sets up the defined set of hydrodynamic coefficients in 
the stack which are necessary and used as global variables for the simulation. After a 
simulation is conducted, the Matlab file “disp res(Xinert,Zbody,t)” plots the overall 
results for the model. This includes all the velocities, translational and rotational 
variables. The model can be divided into seven sections: six degree of freedom 
dynamics, inertial position and euler angles, commands to the vehicle, sliding mode 
controllers, fault detectors, faults event generator, and fins model. Each system will be 


described in subsequent sections. 


B. SIX DEGREE OF FREEDOM DYNAMIC MODEL 

The heart of the simulation Droste is a Six degree of freedom dynamic model 
using hydrodynamic coefficients and math models from the Naval Undersea Warfare 
Center (NUWC) report Hydrodynamic Coefficients and Six Degree of Freedom Model 
for the NUWC UUV. These identify inertial, lift, drag, and added mass parameters. 
Standard equations of motion were employed, but configured to include an “X” rather 
than a cruciform stern plane configuration. Unreported work by Marteno (1997) as a 
summer project solved the equations of motion using Maple, then created a “.cmex” file 
using the ‘C’ compiler in a “unix” platform. The “.cmex” files, appropriate for solving 
for vehicle body frame based inertial and relative velocities, were included in the S- 
function called “model”, while the S-function “inertial” integrates velocities into global 
frame positions. Simulated wave and current programs are also included in the model. 


Other Matlab files in this section include “body_vel”, “crossflow’, and “wavevel.” 
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Inputs were made into the “6dof dynamics” block. Figure 4.2 shows the “6dof 


is the initial surge, “vO” is the initial sway, “w0” is the 
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dynamics” input block. 


initial heave, “pO” is the initial roll rate, “qO” is the initial pitch rate, “rO” is the initia 


yaw rate, and “amp” is the wave amplitude. In the case shown in the figure, the initia 


surge 1s 6 feet per second and the wave amplitude is 2 feet. All other variables are zero. 
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Figure 4.2 Six Degree of Freedom Dynamics Input Block 


; INERTIAL POSITION AND EULER ANGLES 


The inertial position and euler angle model also used well known transformations 


to link body frame velocities to inertial positions. This model produced the translational 


and rotational relationships needed to properly simulate the vehicle. Relevant Matlab 
files in this section include “inertial”, “inertial_eq”, “euler2body6d”, and “predict.” 
Inputs were made into the “Inertial Position and Euler Angles” block. Figure 4.3 
shows the “Inertial Position and Euler Angles” input block. All inputs are made into the 
“S-function parameters” line separated by commas. The first three numbers are the initial 


translational relationships X, Y, Z. The next three relationships, are the initial euler angles 


gy, 8,and y. In the case shown in the figure, the initial Z or depth is 10 feet. All other 


variables are zero. 


Inertial Position and euler angles 
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Figure 4.3 Inertial Position and Euler Angle Input Block 


: COMMANDS TO THE VEHICLE 


Commands to the vehicle can also be altered during the simulation. The three 


commands depth, steering and speed are altered using a step input. 


1. Depth 


The depth command can be altered during a simulation. Figure 4.4 shows the 


“Depth (Zcom) Command” input block. The first line “Step time” indicates the start time 
for the vehicle to perform the maneuver. The second line “Initial value” is the initial 


depth of the vehicle. This input should be the same as the third entry in the “Inertial 


e 


Position and Euler Angles” block. The third line “Final value” is the new commanded 
depth. In the case shown in the figure, at 30 seconds the vehicle is commanded to dive 


to a final value of 20 feet from an initial value of 10 feet. 


3 Depth (Zcom) Command 
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igure 4.4 Depth (Zcom) Command Input Block 


2. Steering 
The steering command can be altered during a simulation. Figure 4.5 shows the 


“Steering (PSIcom) Command” input block. 


Steering (PSicom) Command 
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Figure 4.5 Steering (PSIcom) Command Input Block 


The first line “Step time” indicates the start time for the vehicle to perform the maneuver. 
The second line “Initial value” is the initial course of the vehicle. This input should be 
the same as the fourth entry in the “Inertial Position and Euler Angles” block. The third 
line “Final value” is the new commanded course. In the case shown in the figure, at 10 
seconds the vehicle is commanded to Steer to a final course of 045 degrees from an initial 


course of 000 degrees. 


3. Speed 


The speed command can be altered during a simulation. Figure 4.6 shows the 


“Speed (ucom) Command” input block. The first line “Step time” indicates the start time 
for the vehicle to perform the speed change. The second line “Initial value” is the initial 
speed of the vehicle. This input should be the same as the “uO” entry in the “6dof 


« 


dynamics” block. The third line “Final value” is the new commanded speed. In the case 


shown in the figure, at 40 seconds the vehicle is commanded to increase speed to 10 feet 


per second from an initial speed of 6 feet per second. 
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Figure 4.6 Speed (ucom) Command Input Block 


, SLIDING MODE CONTROLLERS 
The sliding mode controllers for the model processes inputs from the six degree 
of freedom model, the inertial position and euler angle model, and maneuvering 
commands and passes the appropniate signal to the fins for action. Figure 4.7 shows a 


block diagram for the control system. 
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Figure 4.7 21UUV Computer Model 


Relevant Matlab files in this section include “control design’, “ctr_dive”, and 


“ctr_steering.” There are four different controllers: diving, steering, roll, and speed. 


1. Diving 
The input for the dive control system is shown in Figure 4.8. The first two inputs, 
nonlinear gain and layer thickness, were derived from empirical data. The gain matrix 


(K), the s vector, and the B vector were all obtained from the Matlab file 


‘control design. 
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Figure 4.8 Dive Control Input Block 


2. Steering 


The input for the steering control system is shown in Figure 4.9. The first two 
inputs, nonlinear gain and layer thickness, were derived from empirical data. The gain 


matrix (K), the s vector, and the B vector were all obtained from the Matlab file 


‘control_design.” 7 


at a control 
jC u=Kx-(sB}1" 


ragga reer, +? 234.3 


STAD ASAD DPD eR OTR Te 
PO eee PTF PRN e ORR ELE 
"APPOIID SG PPD Oe 
PAIGP OPE PPD GF 
p= $e cape ee 
coasgrte 
ios Site ee 
Poa BOD op ome 


~<a = ~ . = = 4 
peters Sin oecsseies ‘ 4 ‘errand 2g rer sae Agee sg ten sy SMe ARTO Pee Pc BOL bier PD *PDT 
Fete ? O84 Cee = Peete paces ¢RtSOPER OAR DB PIICIP ERIS LEIP. 
Celso pine ° esr se (ret te Vath aA BAAe ae it pms 27 ip ge pain, te 3e Gog: 


 ntdbabpneed DAN AN LOCO LILAC LOE AONE OOOO LODE LOOROL ADDN DAN ae 


redress Dt rertecsa rears s 
39573 G54 “ a Perel eekhrscarseocse ao 
CARO “cP P POR PS pt eager ne Bn 222-8 
terre re COOOL ELPIL TA DUPPIIE AFR GE COEPL PP IEPEOII LEP FEAL A PPLDLPPPIPILOD 
peek eere 22" 20748 PES GDGs a ag CCESCE CEI IO Dad CAGE anaes Leeann s tea Hens cPdOLPOPLOLL CE 
ARORPOPEEPEEPOLPIELIEAT REPL A LED AGATA? 2 GDA RA Rs CA2e ec BAPE BSAC OE, 
69S ae ea papa e CORDA Rae BREE Ee 49 a Rea pag Gere ees =a sopogar® 
<A ste P APA FPL PRS FIEIDIOROI OS Pero ge B8eB bee coped erro ereren 
FE PROTO TAPED *) PEAPILS LR RIAP LEE $22 2b203 27 emer pare ces aer @ 
FOOT PPIPOLCIORPO PERL DPOGD Ye DOR BA tone AG tC POTO POLIO ERCP CECE. 
geasecassi sane Lt aoe csel ened tae acca re 


GOLA 


SSNS AS 
ered - 


Spite tae hae ts ty eel artnet tess 
ee Rl dla LA AEP ALA a SEP PPEEL OAPI? “ AAS OBP ISERIES FF: PAI ARAM 
APE es PLECEP EE AIEPEDEPDPEA LED EDEL PEL, FEOLPPOEPELG PE PP ELEBPPOPE AL ELA LE, 
Od. SP LAEPACIAESIO LEAL ELLER: ae MAPPOPEBEP CAP EPOE RSA, POPPEPORAPPE A EPC PAP 
SAPBLPOPLE PEP AP ALI PME, ALORLE LOE? Ae COP CAE CO EPRP IPR AIEEE LOTTE 


PPPDEL OPE. 2 Aa PPADS OE, LAPP EM ELAIE PIES EL, 

Sere sg ae Pye EE MPLIOPROPOPAPOP PI POLED? BEPERODLOL ALL 
POLPPALEIPLOPLIL BDL EEDA EPPPSILSPL. TPCPEEPL POLLEPTL PE IPPOPEPEPEP EDT BIDE 
SO APPRPCIDIL POPPA DE LED PLT IL IE PN SM PODPPLEOLIOLL POPPED OIPCEEDLIOPFELEPPID 


Bee ee SE SOE ERE ESTEE SSE SEEESSEESSSSEE PIR EPLD 
PIP) A PPOPPLT IOC COLOLOLOMDDOD EI: PD IRCEOCORAG COTE DICE LIG LOLI PICDD COOL DLO Db bD GOD AD bbb Ot BODIE ROOT ERIC TD IOP EI OOOOITO TODS Ot IODA 


ch 
eleSSNS NAN 08 0.0 0.5.6 


WANS ENS he 
AVN REV EON Or ee, 


sf 
ATA 


LAL DELL DLT OL ILIA TELE RELALLULL LL OSLER CLE LEMLEL AL SASL OLALOELECGOMMIDL AGE OAMELLERE LE LOLI SLLLOE NS sagese: 
4 < a: LOR SPIL LLM. Pe PPLEEEM MAAS. S0e. FLEE PEP PALA PO HER ELL PERO Ah. AMPLE PPPAPPEEL PUREE aa 
Fo tab eupapenoeerMeca rege nng BCONIOGe: PORE ROOD ISOS LIL ODIOGED ID DED RE EERO C BSED D OOD EDO DIE EPPS AER APILOSA a8 
AAG Lan eLee emda sews een tt sleguAsncs LOAecteeaed ot SOC tl Eb CCA Gud agacereGs ne Gen Gear ear tarsegaseac 
Zee POOPED Pe AAPEL SSSA GAP PS RAIA PTPPEP CELIO PL PLS CLEPLEL PPI SDAP OLD PY Rae SALd PAARRPOPLOP ELIOT A: 
4 FELPOL EEE AFP RIDDLE oF abe oe 7. ef 7 AOPLEDOPOD LL APEEEE BL B* 
POPR*SPPLOLAPER 4 i. 7% oS 44722 PAPPP AED EPL OAAED 
4 pearance +4. 
g Wc APC eOMh ED PRAM DEEO ROUGE TGS 
2Ao 8 Od APe PROB R APO CMP BPE EP LEAT D EOL BLED OSD GPOPAD Be Abbi De AEDGLOR OGL OPE PP AEE AL APE: sae 
APEPELPLPP PPP PP IS ALOEFOPOED SE Soo a aoa esp rea eaotebeteereed Bee eee ets cecetnegr cctcacaee caaages ce ccupanents 


” (0.5762,-1.67.0) 


SELLE filet PE Le REE tN TSR ELEC ELEL COLLIS EET OL EIN CULL TEST PO LC LAL AOR Cee LLC LaaL SARC ene n LI Lereseai LEM Mei LL ails 
Loa te C OA COLES Ae Ce Sete AO COA Ol AS ete eee e A POO SAIC LE Ss COO POC LOCO Clee LOL LOCLen Gian LOO LeeLee eeu ts 
7 SATII AO OLE ee L CAL Cease COOGEE REE: » 

SELLPEEPPPIP ES / 2 PP MPPLIFOLE *PPPPLIPFISL PLD COAPAG EDD AEC 

PLPOGPALP EL LL? LPO P PAPEL PIVECPEP PEPE ACUPLPE CPE OCPPFOL ELS PL Ge Ponb OO oe 

<P PEDPEPPLPELIL LOD AG EERE LEELA EPLPLLEPP PS ft PRECEESELELL OPIBEE DOPE PDEA POS PEELPEP, 

SPLPAOREPLA PLD LILEPLIPL POLI PIED LOL LLEBLSICED OPIL PSA PEDLEPPOPFEPDLLLE GH Ae LO 

SEERPEPIIO ES’ Bag ti 23222052 0 oagagmean aes ate PAR ERED? ERLOPE ED ILE A: 


54:0.8804:0, 4740) 


SSR ag so tou meeee 
ELEC LL PPLPLTAL 
SRSA SPALL LLC LAL IL DENN O IDE ESR MOA CMLL ILE LRELL LAL ELAM AL LOLOL CL BEE SOLAS L ICM LOL IDLE 
P°GPIPLPPIS LI OI PLPDIP IL OP. PSII RO PILED SLI BLPLISOCEP TELE OLOL PODPPLAG! PLOIPCL COP IPLPCRLIOLDS, SOLIOPL PPP POD POPPI ILE ED, 
POCOL ODES ELEC FOOL O ERE OET EDIE ROP OO DOC COUP MO DIOR ORDO G COG RIGOR OG POLO IE ORU DORE EEDLIDE 
RAPA A ARDC RAR RAAT ROA PRIOR RRA ALAA A RRNA AAA RRO UNR hg ORLA AKA 
AAPL OPPS EPPS OI EIPL ALLE PPLO LEAL ISLE DIP A PEL IILL LDPE PPPL ALIA PE ELL DSA PPPS PLE EPLPP PROC EPPLPI OD PG APLLP PIPL OLS 
POLPEOELSPLOLAS Pv BOOLLEPAPPPLOLIILP EL  LIPLEE PE ETORPLR COE OMEPDPIACG PLP PEEYPEG ES aececi sarees ce zege, 
LOR SCE CTE CC AORN LLEILL ECL EL O er ALA LA OP LOOSE LOLI A CAL OA IG DIG IIRL OGL OLROE poccseoessergeh eer roeees 
COCCI AA GOLA LO ones SALLE EPLI PEL A 2PLELEPIPLI LIL PEELE LOPPPOPLOPPODSLIPE PL ALD Gaeno. 
of Mee CPOAP APOE aco Ce HORI ORR OMD DORE GR OO RO DO OGE anes 
“ee PEOPLE PDPLIILISI DY DP PEPER LEGS COED! br PPLEPOPE IE OD 2. 
SOE, FOIE OONOLE, Lebo pb decent Coon SARNIA O LATE LODO DPD te 


wee Mert sccectsseeeeareerersts eet aaas seas te ee/ee eae es nasoce 


ANS 


Cee eves Peeve 


VSP ne ae een CIO Ee aL ele Geet GCL ee eee eens ete ee 6 Pets 
SELON DLT ASLE MOY LEE PLEA PPOOCLL LOS LE DOO EP EOP LEO AG. LPPAIL A 
P PLOL I PPL OEP EP PPL PEREOOL PIED POPPI MIDAS 
"4 PIR APL PEA OP OL LEP Od PLL P EAE PE PP IAL CAN < ie 
DE MBIGE SOO C COOP oo age eee OOM OE EO IE OIL ER eneetse rupee nmeereeeee: 
AERA ree PEP BLOAT ECPL ELA e 22 ELE LAL ILEP BEALE SALES E00 
I SRG AAAS 


% . “jas a - mame, wr oe see 
Poe ee ee LOM, af a! SOD e ree ee reo sess ‘ee woh a es *LOFAE FPPTOE LIE 
eZ AeA rPGIL PGA? Lees - PLEAS. err arren, 
PPOERPELLEPBEPCL PE OLELPL ODE 2. PORPLPPLOEEP LOE EDP EP EPLEDLOEE TLE, POPCOLOED PPP E. Ae EPO OP PEPORED LED, PIP: 
LPOPAPALID OT PEO Agee Ae. Oe BLIRPBRT PARMA LERAABEB EL AAA L 0b PAP IOPPL IL FOS, te MOP MEER A 0h LPAI LOAD 
* *2e lesa ae PPPOE, +e PPPOC OE i A MPPE PEL PLEA OPE, TG POTOPLPE tay TAO AORPO OE ae TADGP APE AEM 
Se EE EDIG Pe OOP LAP ES oo BGA Oe 1 oa C0 ee PAE T PE Pa BIOCREL 
+2 27 ert Ere eee. =A te AOC AL soaree a oe eres CF. FRA CEE De 2277 ee rer em ee) 
2 2 gas: a7 2e-" i aap 132% WN Beaega en pe ee ae Senseo" 
750" , tate’ = 


- Og SE 





Figure 4.9 Steering Control Input Block 


3. Roll 


The input for the steering control system is shown in Figure 4.10. 
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Figure 4.10 Roll Control Input Block 


The first two inputs, nonlinear gain and layer thickness, were derived from empirical 


data. The gain matrix (K), the s vector, and the B vector were all obtained from the 


Matlab file “control_ design.” 


4. Speed 


The speed control system is shown in Figure 4.11 as a block diagram. 
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Figure 4.11 Speed Control System 


F. FAULT DETECTORS 


The fault detectors on the model take inputs from the output of the controller, 
compare that value with a threshold value, and determine whether a fault has occurred. 
The detector passes two signals, the original and a fault/no fault signal, for future 


implementation into a higher level control. 


Four types of fault detectors are used in the model: servo error, fins deflection, 


wave motion, and observer residuals. 


a3 


1. Servo Error 

The servo error detector takes the “sigma” output from each of the four 
controllers, compares that value with a threshold value, and determines whether a fault 
has occurred. The two signals passed on for future implementation to a higher level 
control are “esignals” and “edetect.” The Matlab file “disp signals” plots the results 


from this detector. Figure 4.12 displays the servo error detector block diagram. 
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Figure 4.12 Servo Error Detector Block Diagram 


2. Fins Deflection 

The fin deflection detector takes the “commands” from each of the four 
controllers and converts the outputs into a fins deflection sensor by running through two 
gain matrices. That output is compared with a threshold value to determines whether a 
fault has occurred. The two signals passed on for future implementation to a higher level 
control are “fsignals” and “fdetect.” The Matlab file “disp f’ plots the results from this 


detector. Figure 4.13 displays the fins deflection detector block diagram. 
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Figure 4.13 Fins Deflection Detector Block Diagram 


3. Wave Motion 

The wave motion detector is necessary for the model due to the near surface 
operation of future UUVs. The wave motion detector takes the “sigma” output from each 
of the four controllers, compares that value with a threshold value, and determines 
whether a fault has occurred. The two signals passed on for future implementation to a 
higher level control are “wve_signals” and “wve_detect.” The Matlab file “disp_signals” 
plots the results from this detector. Figure 4.14 displays the wave motion detector block 


diagram. 
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Figure 4.14 Wave Motion Detector Block Diagram 


4. Observer Residual 
The design of observer residual fault detectors is discussed in depth in Chapter III. 
The purpose of this subsection 1s to describe how inputs are made into each of the three 


observers: diving, steering and roll. 


a, Diving 

Figure 4.15 shows the “Diving System Observer” input block. The A, B, 
C and D matrices, as described in Chapter III, were obtained from the Matlab file 
“dive_obs_ des.” Commas separate all inputs made to the “Initial Conditions” line. The 
four entries are w,, the heave velocity of the vehicle relative to the water, q, the pitch rate, 


6, the pitch angle and Z, the depth. In this case shown in the figure, the initial depth is 10 
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feet and all other variables are zero. The Matlab file “disp obs dive” plots the results 


from this detector. 
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Figure 4.15 Diving System Observer Input Block 


b. Steering 

Figure 4.16 shows the “Steering System Observer” input block. The A, B, 
C and D matrices, as described in Chapter II], were obtained from the Matlab file 
‘steer _obs_ des.” Commas separate all inputs made to the “Initial Conditions” line. The 
three entries are v,, the sway velocity of the vehicle relative to the water, 7, the yaw rate, 


and y, the yaw angle. In this case shown in the figure, the initial depth all variables are 


zero. The Matlab file “disp _obs_steer” plots the results from this detector. 
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Figure 4.16 Steering System Observer Input Block 


c. Roll 


Figure 4.17 shows the “Roll System Observer” input block. The A, B, C, 


and D matrices, as described in Chapter III, were obtained from the Matlab file 
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‘roll_obs_ des.” Commas separate all inputs made to the “Initial Conditions” line. The 
two entries are p, the roll rate, and @, the roll angle. In this case shown in the figure, all 


variables are zero. The Matlab file “disp obs roll” plots the results from this detector. 


+ RollSystem_Observer 
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Figure 4.17 Roll System Observer Input Block 


: FAULTS EVENT GENERATOR 


The faults event generator can change of the status of any of the four fins on the 
model at any time. Figure 4.18 shows the “Faults event generator” input block. The first 
line “S-function name” refers to the Matlab file “faultsgen.” This file was used to create 
the faults event generator. The second line “S-function parameters” has two matrix 
inputs. The first is a four by one matrix, which indicates the initial condition of the fins. 


Each element in this matrix represents a fin. The first element is fin 1, the second fin 2, 


> ole = 


etc. The numbers for each element indicate a specific “condition. indicates the fin 


is in normal operation, “2” indicates that the fin is stroke limited (can only move to 0.25 


radians instead of the full 0.4), “3” indicates that the fin is loose (ineffective, stays at 


zero), and “4” indicates that the fin is stuck (stays at 0.4 radians). In the case shown in 


the figure, all four fins are initially operating normally. 
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Figure 4.18 Faults Event Generator Input Bloc 


The second input is a three column matrix. The first column represents the time 
of the action, the second column represents the fin number, and the third column 
represents the “condition” of the fin. This matrix can have a maximum of six rows. In 
the case shown in the figure, at 30 seconds a stuck rudder was imposed to fin 1 and at 60 


seconds a loose rudder was imposed to fin 2. 


: FINS MODEL 


The process of how a fault is added to the model is shown in Figure 4.19. 





Figure 4.19 Fault Addition Process 


Commands from the diving, steering, and roll control systems are inputted in the 
“control to fins” block. In this block, the commands from each of the control systems are 
multiplied by a gain matrix and converted into deflections for each fin (6), d>, 63, dy). 

The fins model takes inputs from the fin deflections and faults events generator 


and adds the signals together to produce 67, dp, 63, and dy. Figure 4.20 shows a block 


diagram of the fin model. 





faultsgen df1, 5f2 
Mux4  5f3, 5f4 


Figure 4.20 Fins Model Block Diagram 
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By entering the fin subsystem, parameters can be changed for each fin. Figure 
4.21 shows the “fin1” input block. The first line “Max fin stroke” refers to the maximum 
turning capability of the fin. The second line “limited fin stroke [fault type 2]” refers to 
the maximum turning capability when at stroke limited condition. The third line “stuck 
fin [fault type 4]” refers to the position of the fin when at a stuck fin condition. In the 
case shown in the figure, the maximum fin stroke is 0.4 radians, the limited fin stroke 


condition is 0.25 radians, and the stuck fin condition is at 0.4 radians. 
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Figure 4.21 Fin Input Block 


After exiting the fins model block, dn, 62, ds, and dy are multiplied by another 
gain matrix, “fins to control”, to produce the dive, steering, and roll commands for input 
into the 6dof dynamics model. 


An example of a dive command, 6,, is shown in Figure 4.22. The four fins in an 


“xX” configuration are shown from a stern aspect. The arrows represent the force 


4 


direction of each respective fin. In this case, all horizontal forces cancel out and the 


remaining forces add up to produce an upward deflection. 


Figure 4.22 Dive Command, 6;, Example (Stern View) 


Figure 4.23 shows an example of a steering command, 6;. In this case, all vertical 


forces cancel out and the remaining forces add up to produce a deflection to the nght. 
~ 6 
a, fF 
=> 
3 2, 


Figure 4.23 Steering Command, 6,, Example (Stern View) 
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Figure 4.24 shows an example of a roll command, 6,. In this case, all horizontal 


"and vertical forces are canceled out but a clockwise moment is produced. 


Aes 4 
- 85 
S$ ~~ 2 


Figure 4.24 Roll Command, 6,, Example (Stern View) 
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V. SUMULATION RESULTS FOR SPECIFIC CASES 


This study uses the 21UUV computer model to simulate actions of an actual 
autonomous underwater vehicle in shallow water operations. To test the effectiveness of 
model based observers for fault detection, five specific areas of study were chosen: 
robustness of “X” fin configuration, weight and buoyancy mismatch, detection of fin 
faults in the presence of waves, fin fault detection using maximum likelihood analysis, 


and control at slow speeds. 


A. ROBUSTNESS OF “X” FIN CONFIGURATION 

This study was conducted to determine if the “X” fin configuration of 21UUV 
provides a level of redundancy, which the control systems (diving, steering, roll, and 
speed) can use to reconfigure the autopilot system. In the 21UUV model, fin 1 is 
redundant with fin 3 and fin 2 is redundant with fin 4. 

Scenario 1: fin fault followed by steering command with weight mismatch. In 
the first scenario, the vehicle is moving forward at a speed of 8 feet per second. The 
depth of the water column is 30 feet and the vehicle is at 10 feet. The vehicle is on a 
course of 000. For this simulation, there are no waves. 20 seconds into the simulation, a 
stuck #1 fin fault was imposed with a level of 0.4 radians. At 50 seconds, the vehicle is 
commanded to steer to 045. The simulation was run for 100 seconds. Figure 5.1 shows 
an “X-Y” plot of the vehicle’s path trajectory. This plot shows that despite a fault to fin 
1, the vehicle was successfully able to turn to 045, and crudely maintaining heading in 


spite of the fault. 
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Figure 5.1 Vehicle Path for Scenario 1 


A closer examination of the turn can be determined by the yaw angle, y. Figure 


5.2 shows that the vehicle was able to maintain a course of 000 after the fin fault occurred 


at 20 seconds. 
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Figure 5.2 Yaw Angle for Scenario 1 
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When the turn was commanded at 50 seconds, the vehicle turned and steadied up on 045 
by 85 seconds. Note that the vehicle actually turned to 065 at 75 seconds before reaching 
a steady state. 

To see how fin 3 compensated for the fault to fin 1, a plot for fin response versus 
time is examined. Figure 5.3 shows the fin response for fins 1 and 3. The response of the 
fins is equal in magnitude until the fault occurs at 20 seconds. Then, fin 3 adjusts quickly 
to compensate for the fin 1 stuck condition. When the turn 1s executed at 50 seconds, fin 
3 adjusts again to compensate for fin 1, although control dynamics are degraded. 
Therefore, it appears that one fin can be lost on 21UUV without compromising the 


steering capability of the vehicle. 
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Figure 5.3 Fins 1 and 3 Response for Scenario 1 


49 


To determine the fault detection capability of the observers, the steering and roll 
residuals will be examined. For the steering observer, the residuals of interest are the 
sway velocity of the vehicle relative to the water, v,, (sensed by acoustic doppler) and, the 
yaw rate, r. Figure 5.4 shows the steering observer residual response for v, and r. Note 
that both residuals show a response for the fin fault at 20 seconds, but both residuals also 
show a response of equal or greater magnitude for the turn at 50 seconds. The steering 
observer residuals can not effectively distinguish a fault from a turn. This is due to the 
fact that fin faults are additive in the steering control system. Thus, steering observer 
residuals are seen to be responsive to maneuvering conditions, in spite of simple theory, 


and may only be used to detect faults during nonmanuevering conditions. 
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Figure 5.4 Steering Observer Residuals v, and 7 for Scenario 1 


The roll observer residual of interest is the roll rate, p. Figure 5.5 shows the roll 


observer residual response for p. Note that the residual shows an immediate response for 
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the fin fault at 20 seconds. The magnitude of response for the turn at 50 seconds is less 
than for the fin fault. Although fin faults are additive in the roll control system, the roll 


command is always set back to zero. 
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Figure 5.5 Roll Observer Residual p for Scenario 1 


Scenario 2: fin fault followed by diving command with weight mismatch. In the 
second scenario, the vehicle is moving forward at a speed of 8 feet per second. The depth 
of the water column is 30 feet and the vehicle is at 25 feet. The vehicle is on a course of 
000. For this simulation, there are no waves. 20 seconds into the simulation, a stuck #2 
fin fault was imposed. At 40 seconds, the vehicle is commanded to “dive” to 0 (surface). 
The simulation was run for 100 seconds. Figure 5.6 shows a depth plot of the vehicle. 


This plot shows that despite a fault to fin 2, the vehicle was successfully able to surface. 
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Figure 5.6 Depth of Vehicle for Scenario 2 


A closer examination of the “dive” can be determined by the pitch angle, @ 


Figure 5.7 shows the pitch angle versus time. 
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Figure 5.7 Pitch Angle for Scenario 2 
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The vehicle was able to maintain depth at 25 ft after the fin fault occurred at 20 seconds. 
When the “dive” was commanded at 40 seconds, the pitch angle increased to 25 degrees. 
After surfacing at 60 seconds, the vehicle returned to the steady state pitch angle before 
the maneuver. 

To see how fin 4 compensated for the fault to fin 2, a plot for fin response versus 
time is examined. Figure 5.8 shows the fin response for fins 1 and 3. The response of the 
fins 1s equal in magnitude until the fault occurs at 20 seconds. Then, fin 4 adjusts quickly 
to compensate for the fin 2 stuck condition. When the “dive” is executed at 40 seconds, 
fin 4 adjusts again to compensate for fin 2. Therefore, it appears that one fin can be lost 


on 21UUV without compromising the diving capability of the vehicle. 
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Figure 5.8 Fins 2 and 4 Response for Scenario 2 


To determine the fault detection capability of the observers, the roll observer 


residuals were examined. The roll observer residual of interest is the roll rate, p. Figure 
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5.9 shows the roll observer residual response for p. Note that the residual shows an 
immediate response for the fin fault at 20 seconds. The magnitude of response for the 
“dive” at 40 seconds is less than for the fin fault. Thus, by setting an appropriate 


threshold, a fin fault can be distinguished from a maneuver. 
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Figure 5.9 Roll Observer Residual p for Scenario 2 


Scenario 3: low speed with two fin faults. In the third scenario, the vehicle 1s 
moving forward at a speed of 4 feet per second. The depth of the water column is 30 feet 
and the vehicle is at 10 feet. The vehicle is on a course of 000. For this simulation, there 
are no waves. 20 seconds into the simulation, a stuck #1 fin fault was imposed. At 40 
seconds, a stuck #3 fin fault was imposed. The simulation was run for 80 seconds. 
Figure 5.10 shows a depth plot of the vehicle. This plot shows that after the second fin 


fault, the vehicle is unable to maintain depth control. 
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Figure 5.10 Depth of Vehicle for Scenario 3 


Figure 5.11 shows the vehicle is also unable to maintain steering control after the second 


fault. 





Figure 5.11 Vehicle Path for Scenario 3 
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To determine the fault detection capability of the observers, the steering and roll 
residuals were examined. For the steering observer, the residuals of interest are the sway 
velocity of the vehicle relative to the water, v,, and, the yaw rate, r. Figure 5.12 shows 
the steering observer residual response for v, and 7. Note that both residuals show a huge 


response after the second fin fault at 40 seconds. 
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Figure 5.12 Steering Observer Residuals v, and r for Scenario 3 
The roll observer residual of interest 1s the roll rate, p. Figure 5.13 shows the roll 


observer residual response for p. Note that the residual shows a response after the first 


fin fault and a huge response after the second fin fault. 
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Figure 5.13. Roll Observer Residual p for Scenario 3 


B. WEIGHT AND BUOYANCY MISMATCH 

Due to the inherent difficulty of running the 21UUV with perfectly neutral 
balance, a study was conducted to determine if the diving observer residuals could detect 
a weight-buoyancy mismatch. 

The first step in this study was to evaluate a vehicle where there was no weight- 
buoyancy mismatch (weight equals buoyancy). Scenario 4: Straight run with no weight- 
buoyancy mismatch. In the fourth scenario, the vehicle is moving forward at a speed of 8 
feet per second. The depth of the water column is 30 feet and the vehicle is at 10 feet. 
The vehicle is on a course of 000. For this simulation, there are no waves. The 
simulation was run for 50 seconds. Figure 5.14 shows a depth plot of the vehicle. As 
expected, this plot shows that the vehicle is able to hold depth when there is no weight- 


buoyancy mismatch. 
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Figure 5.14 Depth of Vehicle for Scenario 4 


When examining a model based fault detector, the diving observer residuals of 
interest are the heave velocity of the vehicle relative to the water, w,, and the pitch rate, g. 
Figure 5.15 shows the diving observer residuals w, and g and confirms that there are no 


faults in this scenario. 
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Figure 5.15 Diving Observer Residuals w, and q for Scenario 4 
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The final step in this study was to evaluate a vehicle where there was weight- 
buoyancy mismatch (weight is 2 percent greater than buoyancy). Scenario 5: Straight 
run with weight-buoyancy mismatch. In the fifth scenario, the vehicle is moving forward 
at a speed of 8 feet per second. The depth of the water column is 30 feet and the vehicle 
is at 10 feet. The vehicle is on a course of 000. For this simulation, there are no waves. 
The simulation was run for 50 seconds. Figure 5.16 shows a depth plot of the vehicle. 
This plot shows that the vehicle is unable to hold the commanded depth of 10 feet. The 


vehicle sinks until reaching a steady state depth of 14 feet. 
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Figure 5.16 Depth of Vehicle for Scenario 5 


A closer examination of the weight-buoyancy mismatch can be determined by the 
pitch angle, @ Figure 5.17 shows the pitch angle for the scenario. Due to the excess 


weight of the vehicle, @ increased to 5 degrees and stayed there when the vehicle reached 


steady state. 
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Figure 5.17 Pitch Angle for Scenario 5 


When examining a model based fault detector, the diving observer residuals of 
interest are the heave velocity of the vehicle relative to the water, w,, and the pitch rate, g. 


Figure 5.18 shows the diving observer residuals w, and g for this scenario. 
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Figure 5.18 Diving Observer Residuals w, and q for Scenario 5 
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As shown, the residual w, immediately responds to the heaviness of the vehicle. The 
residual g responds a bit slower, but still detects the heaviness of the vehicle. Thus, it 
appears that w, is more responsive than g. By setting a threshold value on w, of 0.3 feet 


per second, any weight-buoyancy mismatch on the vehicle should be detected. 


C. DETECTION OF FIN FAULTS IN THE PRESENCE OF WAVES 

This study was conducted to determine if model based observers could be used to 
distinguish fin faults from wave disturbances. A properly designed model based observer 
should be unresponsive to wave disturbances, but should react to an actuator fault. 

Scenario 6: fast speed with two fin faults and waves. In the sixth scenario, the 
vehicle is moving forward at a speed of 10 feet per second. The depth of the water 
column 1s 30 feet and the vehicle 1s at 10 feet. The vehicle is on a course of 000. For this 
simulation, the wave amplitude 1s set at 2 feet and the waves are coming from 000. 20 
seconds into the simulation, a stuck #1 fin fault was Tipoee At 60 seconds, a stuck #2 
fin fault was imposed. The simulation was run for 100 seconds. Figure 5.19 shows a 
depth plot of the vehicle. This plot shows that the vehicle 1s affected by the wave 
disturbance. The first fin fault has little influence since the vehicle 1s able to reconfigure. 


The second fin fault causes the vehicle to lose depth control. 
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19 Depth of Vehicle for Scenario 6 
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Figure 5 


Figure 5.20 shows the vehicle path. This plot shows that the vehicle loses 


steering control after the second fault occurs. 
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Figure 5.20 Vehicle Path for Scenario 6 
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To determine the fault detection capability of the observers, the roll observer 
residuals were examined. The roll observer residual of interest is the roll rate, p. Figure 
5.21 shows the roll observer residual response for p. Note that the residual shows no 
response from 0 to 20 seconds. This means that the observer residual is not affected by 
wave disturbances. At 20 seconds, a response occurs during the fault to fin 1. At 60 
seconds, an even bigger response occurs during the fault to fin 2. Therefore, by setting 
an appropriate threshold, a fin fault can be distinguished from wave disturbance. A 


threshold level of 3 degrees per second on p should detect a fin fault. 
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Figure 5.21 Roll Observer Residual p for Scenario 6 


D. FIN FAULT DETECTION USING MAXIMUM LIKELIHOOD ANALYSIS 
In order to more clearly distinguish faults from maneuvers in residual signals, 


likelihood functions are used to provide probability ratios. By cross correlating the roll 
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rate observer residual signal with an auto-correlated expected unit step fault response, the 
likelihood of a fault or maneuver is more clearly distinguishable. The likelihood function 


is described by Peng, et. al., (1997): 


Si 2 

Pr <a 

L(i)==BR| SA 
J 


a 
k=l 

Li) is the ratio of probability that vis caused by a step change to probability that v is 
caused by non-specific inputs. y; is the residual signal at 7 df. ¢; is the unit step input 
response of the filter. A/is defined as the smoothing window. / goes from 1 to M. 

Scenario 7: medium speed run with steering command followed by a subsequent 
fin fault. In the seventh scenario, the vehicle is moving forward at a speed of 8 feet per 
second. The depth of the water column is 30 feet and the vehicle is at 10 feet. The 
vehicle is on a course of 000. For this simulation, there are no waves. 20 seconds into 
the simulation, the vehicle is commanded to steer to 045. At 60 seconds, a stuck #1 fin 
fault was imposed. The simulation was run for 100 seconds. 

Figure 5.22 shows the plain roll rate observer residual signal for the scenario. The 


amplitude ratio between the fin fault and the steering maneuver is approximately 5 to 1. 
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Figure 5.22 Roll Observer Residual, p, for Scenario 7 


Figure 5.23 shows the roll rate observer residual signal after being run through a 


likelihood filter. The amplitude ratio between the fin fault and maneuvering response is 


now approximately 20 to 1. 
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Figure 5.23, Maximum Likelihood Analysis on Roll Observer Residual, p 
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Thus, by using maximum likelihood analysis, the difference between a fin fault 
and maneuvering signal is clearly more distinguishable. A proposed threshold can be set 


for the detection of a fin fault at 0.3. 


E. CONTROL AT SLOW SPEEDS 

In the course of conducting simulations, the vehicle has shown degrading control 
as speed decreases. This study was conducted to determine why control problems arise 
when the vehicle 1s forced to go less than 6 feet per second. 

Scenario 8: Slow speed run with command to steer with weight-buoyancy 
mismatch. In the eighth scenario, the vehicle is moving forward at a speed of 4 feet per 
second. The depth of the water column is 30 feet and the vehicle is at 10 feet. The 
vehicle is on a course of 000. For this simulation, the wave amplitude is set at 2 feet and 
the waves are coming from 000. At 20 seconds, the vehicle is commanded to steer to 
045. The simulation was run for 80 seconds. Figure 5.24 shows an “X-Y” plot of the 
vehicle. This plot shows that the vehicle turns very slowly. At slow speeds, the vehicle 


is very slow to respond. 
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Figure 5.24 Vehicle Path for Scenario 8 


Figure 5.25 shows a depth plot of the vehicle. This plot shows that the vehicle is unable 


to maintain depth when turning and actually surfaces at 67 seconds. 
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Figure 5.25 Depth of Vehicle for Scenario 8 
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Due to the lack of speed, reaction forces by the fins are not as great as at higher 
speeds. Figure 5.26 shows the fin response with respect to time for all four fins. Note 


that all four fins are near or at saturation. 


Fin 1 Response 
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Fin Position 
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Figure 5.26 Response of all 4 fins in Scenario 8 
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Since the fins are forced to use so much of their authority to maintain depth at 
slower speeds, a simple maneuver like a turn becomes difficult. There 1s not enough fin 
authority left to maintain depth and turn at the same time. Figure 5.27 shows percent fin 
authority versus speed for the 21UUV at a 2 percent weight-buoyancy mismatch. 


Fin Authonty versus Speed of Vehicle 
100 ee i 


Percent Fin Authority (%) 





Speed (ft/sec) 


Figure 5.27 Percent Fin Authority versus Speed of Vehicle Expended for 2% mismatch 
of Weight and Buoyancy 


A complete list of simulation runs of the 21UUV model is included in Appendix 
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VI. CONCLUSIONS AND RECOMMENDATIONS 


A. CONCLUSIONS 

The overall problem of fault detection for 21UUV is complex. Thus, the 
detection of faults was limited in this study to the primary subsystems of the vehicle, 
including the diving, steering, roll control and speed control systems. Many current fault 
detection schemes use static signals. This is done by using “limits and trends” analysis. 
For dynamic signals, such as a stuck or loose fin, the transient nature of the signal makes 
limits and trends analysis invalid. Dynamic fault responses can be obtained from servo- 
error and model based observer residuals. 

Model based observer residuals detectors for the diving, steering, and roll control 
systems were designed and implemented in the Simulink model of the 21UUV. In the 
course of conducting simulations of the 21UUV model, numerous vehicle behaviors were 
validated. In addition, the model based observer residual detectors have been found to be 
useful for the detection of fin faults on the vehicle even though they also respond to 
disturbances and unmodelled dynamic inputs from maneuvering. 

The “X” fin configuration of 21UUV provides a level of redundancy, which the 
control systems (diving, steering, roll, and speed) can use by automatic reconfiguration. 
Fin 1 is redundant with fin 3 and fin 2 is redundant with fin 4. By implementing normal 
autopilot control systems, a fault to fin 1 is compensated to some degree by fin 3 without 
compromising the diving and steering capability of the vehicle. However, a more serious 
condition arises particularly at low speeds when multiple “faults” are present. Faults 


considered were weight-buoyancy mismatch and multiple fin faults. 
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Due to the inherent difficulty of running the 21UUV with perfectly neutral 
balance, weight and buoyancy mismatch was studied. By setting appropriate threshold 
levels, the diving observer residuals for heave velocity relative to the water, w,, and the 
pitch rate, g, can detect a weight-buoyancy mismatch on the vehicle. 

Model based observers may be designed to distinguish fin faults from wave 
disturbances and fin faults from maneuvering responses. It appears that fin faults can be 
detected most reliably using the roll observer residuals. Likelihood tests on the residual 
responses provide further ease of setting detection thresholds. By setting appropriate 
threshold levels, the roll rate, p, can quickly and robustly determine a fin fault without 
corruption from wave disturbances or maneuvering responses. The steering observer 
residuals can not as effectively distinguish a fault from aturn. This is due to the fact that 
fin faults are at times indistinguishable from the problems of fin saturation which is in the 
steering control system. Thus, steering observer residuals can best be used to detect 
faults during nonmaneuvering conditions. Although fin faults are additive in the roll 
control system, the roll command is always set back to zero, eliminating the direct 
excitation of residuals by maneuvers. 

Control problems arise when 21UUV is forced to go less than 6 feet per second. 
With a weight-buoyancy mismatch, the vehicle is unable to maintain depth when 
steering. This is due to the fact that too much fin authority is being used at these lower 
speeds. 

This study has proved that model based observer residuals may be used for the 
detection of fin faults and weight-buoyancy mismatches regardless of wave disturbances 


and maneuvering responses, but ultimately, there is a mismatch coupled with more than a 


ie 


single fin fault that will render the vehicle uncontrollable, aggravated with any weight- 


buoyancy mismatch. 


B. RECOMMENDATIONS 
The following recommendations are made in the continuation of this study: 


- Use the observer residual for roll rate, p, with maximum likelihood filter as the 
primary input to fuzzy inference system for detection of fin faults. 


- Use a maximum likelihood filter on the model based observer residual signal to 
get a more distinct difference between maneuvers and fin faults. 


- Study the possibility of using diving, steering, and roll observer residuals to 
detect subsystem (diving, steering, and roll control) faults. This should be done in 
conjunction with other fault detectors (Servo-error, fins-deflection, and wave 
motion) and tied together via fuzzy inference system. 
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APPENDIX A. MATLAB FILES FOR MODEL BASED OBSERVER DESIGN 


This appendix contains Matlab files for the design of the diving, steering, and roll 


control model based observers, respectively. 


aS 


dive obs _des.m 


$ This program was used to design the 21UUV diving observer 


$ This program is computed in feet/sec/lbf units 

$ the depth controller for a submersible vehicle in forward 
$ motion is designed and computed based on SMC methods. 

% Nondimensionalized version of Hydrocoefficients are used 
$ and reconstituted into dimensional form. here rho=1.94;L=20; 
$ Time is in seconds. V is in feet / sec. 

clear 

V=6; 

t0=0; 

EE—s00; 

rho=1.94;L=20.5; 

$ 

$ Vehicle nondimensional parameters for the DSRV 

$ diving response 

$ 

$ this sets up the diving smc design 

% 

% open loop system 

% 


m=88.9518;%slugs 

ITy=2632.47;% dimensional already 
LTyy=Iy; 

Me=— i) (e-08°0. 5° rho*V* L*4; 
Madot=—7 .504e—047*0.5*rho*L*5; 
Mw=6.746e-03*0.5*rho*V*L%*3; 
Mwdot=-1.753e-04*0.5*rho*L%*4; 
Ma=-2 ih Ge-03"70. 5" Eno*V-2*i1,"3; 
Mth=0.02*m*32.2; 

ZG=—2 0596-05 "0.5* rho*V*L”*3; 
ZqGaoe—— i. / >3e-04" 0.5" rho*L*4; 
ZW==7).4066-05"70. 5° rho VL 2; 
Zwaor—= i, 04 le=027 0.5" 1pno”* L’ 3; 
Zd-—4-206e—-032 02 a7 rho Vo 2*L* 2; 


& (ND time is L/V = 20/4 = 5 seconds) 


MM= (m= Z2waoe),-2q0e0e.0,0;-Mwdot, (lvy=Mqdot) ;,0,0;070,1,0,0,0,0,1); 
APS — (Zw, (Aqtme vj 0, Ur Mw Mic, Mth, 0; 0, 1,0,0;1,0,—-V, 0]; 

BB [ZdsMd;0;; 0] 

A=inv (MM) *AA; B=inv (MM) *BB; C=eye (4,4) ;D=zeros (4,1); 

{num, den) =ss2tf(A,B,C,D) ;p=roots (den) ; 


% desired closed loop poles for sliding are [-0.4,-0.41,-0.42,0]; 


® k=place(A,B,(-0-647-0°5 86417 -027,0).)'; 
K=place (A,B, [-054,-0.4 17-05 42.0) )-; 
> K—-place (A,B, l-0+2,-0.217- 0922701); 


f 
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% closed loop dynamics matrix 
Ac=A-B*k; 

[m,n]=eig(Ac'); 

s=m(:,4); 

Critical Speed for Input reversals 


Wie SB le (AZ ee ee ee ee) 2) SA (all) 8 2 


Simulation of closed loop response 


cP &® CP HD HW oP 


C=eye (4,4); 
$ Observer Gain Matrix 


$K=lge (A, eye (4,4),C,1,eye (4,4) .*100) ; 
h—oibace (ye 71(-0.2,-0.2.1,-0.22,-0. 24] ); 
$K=place(A',C', [-0.4,-0.41,-0.42,-0.44]); 
Pheplace (A 7c ,[—-0.6,—-0.61,—0.62,-0. 64])); 
eig (A-K'*C) 


$ Dive Observer Design 


Ao=A~-K'*C; 
Bo=[(B,K']; 
Co=-C; 


Do=[zeros (4,1),eye(4,4)] 


end 


a 


Steer obs des.m 


%$ This program was used to design the 21UUV steering observer 


This program is computed in feet/sec/lbf units 


the steering controller for a submersible vehicle in forward 


Nondimensionalized version of Hydrocoefficients are used 
and reconstituted into dimensional form. here rho=1.94;L=20; 


% 
% 
$ motion is designed and computed based on SMC methods. 
% 
% 
3 


Time is in seconds. V is in feet / sec. 


clear 
V=6; 
t0=0; 


t£=300; 


rho=1. 
% 


oP OP OP CH CH OH OO 


94;L=20.5; 


this sets up the steering smc design 


open loop system 


m=88.9518;%slugs 
ITy=2632.47;% dimensional alread 


Iz=ly; 


Xud=-1.667e-04; Yvd=-1.041le-02; Nvd=1.753e-04; 


Zwd=-1.04le-02; Mwd=-1.753e-04; Zqd=-1.753e-04; 
753e-04; Nrd=-7.504e-04; Xuu=-8.348e-04; 


Yrd=1. 
Yv=-7. 


406e-03; Nv=-6.746e-03; Zw=-7.406e-03; 


S$increase linear roll damping *10 


Kp=-2. 
Zq=-2. 
Xvv=4. 
Avr=l1. 
Ywp=1. 
Ypq=1. 
aqq=4. 


423e-06; | 

655e-03; Mgq=-1.477e-03; Yr=2,650e-03; 
0O73e-03; Zvp=-1.041le-02; Mvp=-1.753e-04; 
Ol2e-02; Xww=4.073e-03; 

04le-02; Nwp=-1.753e-04; Xwq=-1.012e-02; 
753e-04; Npq=-7.503e-04; Zpr=1.753e-04; 
982e-05; Xrr=4.982e-05; ZDS=-4.216e-03; 


Vehicle nondimensional parameters for the DSRV 
steering response 


Mqd=-7.504e-04; 


Mw=6.746e-03; 


Nr=-1.477e-03; 


Mpr=7.503e-04; 
MDS——2 71 /6e-0s 


XWDS=2 .226e-03; XODS=1.148e-03; XDSDS=-1.429e-03; 
YDR=4 .216e-03; NDR=-2 .176e-03; XVDR=-2.226e-03; XRDR=1.148e-03; 
XDRDR=-1.429e-03; KDA=9.674e-05; XDADA=-2.858e-03; 


KPHI2= 
KPHI4= 


$long. 


O; 
3, J06E—-057 


center of rotation off the body 


Zqaq=-4.121E-03; Mgqagq=-1.770E-03; 
Zwaq=-2.270E-02; Mwaq=-9.487E-03; 


Zw2=-5.917E-02; Mw2=-2.474E-02; 
Zw3=3.750E-03; Mw3=1.132E-02; 
2wW4=1.293E-01; Mw4=5. 463E-02; 
ZwWwaw=-5.643E-02; Mwaw=-1.135E-02; 
Z4qZ2=-4.743E-03; MaZ=— e607 b- 035; 


Zqaw=-3.712E-02; Mqaw=-1.107E-02; 
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$lat center of rotation on the body 
Yvav=-5.643E-02; Nvav=1l.135E-02; 
Yvar=2.2/0E-02; Nvar=-9.487E-03; 
Yr2=-4.743E-03; Nr2=1.607E-03; 


$lat center of rotation off the body 
Yrar=4.121E-03; Neacse=] . 7705-03; 
Yrav=-3.712E-02; Nrav=1.107E-02; 
Yv2=-5.917E-02; Nv2=2.474E-02; 
Yv3=-3.750e-3; Nv3=1.132e-2; 
Yv4=1.293e-1; Nv4=-5.463e-2; 


$Dimensional design for steering autopilot 
Nvdot=Nvd*0.5*rho*L%*4; 
NEaot=Nra*U0.5*rho*L*5; 
MmvGgor—YVG-U.o° Eno L 3; 
Mraot—yYra" 0. 5*rno*L4; 
NV—=NvY* Ono * rho*V*L° 3; 
Ne—Ne- 0. o*rho*V*L 4; 
nvr: OU. 5*rho*Vv* LZ; 
iWr=yrE* 05 5* rno-V* L* 3; 
Ne=-NDR*0.5* rho" V-2"*L* 3; 
vwe=YDR* 0,57 rho*V"2*L° 2; 


MM=[(m-Yvdot) -Yrdot O;-Nvdot (Iz-Nrdot) 0;0 0 1]; 
AA=[Yv (Yr-m*V) O;Nv Nr 0; O 1 QO]; 

BB=[Yd;Nd;0]; 

A=inv (MM) *AA; B=inv (MM) *BB;C=[0,0,1] ; D=0; 
[num,den]=ss2tf (A,B,C,D) ;z=roots (num) ;p=roots (den) ; 


= desired closed loop poles for sliding are [-0.4,-0.41,-0. 42,0]; 


K—place (A,B, [(-0.4,-0.41,0]); 

$ closed loop dynamics matrix 
Ac=A-B*k; 

[m,n]=eig(Ac'); 

s=m(:,3); 


® Observer Gain Matrix 


e—eveiers) ; 
$K=lqe (A, eye (3,3) ,C,1,eye(4,4) .*100); 


meplace (A*,C’*,([(-0.2,-0.21,-0.22)); 
ex—place (A‘*,C', [-0.4,-0.41,-0.42] ); 
$K=place(A',C', [-0.6,-0.61,-0.62]); 
$K=place(A',C', [-0.8,-0.81,-0.82]) 
e1g(A-K'*C) 


% Steering Observer Design 


Ao=A-K'*C; 
Bo=[B,K']J; 
Co=-C; 


Do=(zeros (3,1) ,eye (3,3) ] 


end 
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roll_obs_des.m 
%$ This program was used to design the 21UUV steering observer 


$ This program is computed in feet/sec/lbf units 

$ the roll controller for a submersible vehicle in forward 

%$ motion is designed and computed based on SMC methods. 

% Nondimensionalized version of Hydrocoefficients are used 

$ and reconstituted into dimensional form. here rho=1.94;L=20; 
%$ Time is in seconds. V is in feet / sec. 


tf=300; 

rho=1.94;L=20.5; 

% 

Vehicle nondimensional parameters for the DSRV 
roll response 


this sets up the roll smc design 


open loop system 


0 & C oP ®H oP OO 


m=88.9518;%slugs 

Ty=2632.47;% dimensional already 

Iz=ly; 

Xud=-1.667e-04; Yvd=-1.041le-02; Nvd=1.753e-04; 

Zwd=-1.041le-02; Mwd=-1.753e-04; Zqd=-1.753e-04; Mqd=-7.504e-04; 
Yrd=1.753e-04; Nrd=-7.504e-04; Xuu=-8.348e-04; 

Yv=-7.406e-03; Nv=-6.746e-03; Zw=-7.406e-03; Mw=6.746e-03; 


$increase linear roll damping *10 

Kp=-2.423e-06; . 

Z2q=-2.655e-03; Mq=-1.477e-03; Yr=2.655e-03; Nr=-1.477e-03; 
Xvv=4.073e-03; Zvp=-1.041le-02; Mvp=-1.753e-04; 

Xvr=1.012e-02; Xww=4.073e-03; 

Ywp=1.04le-02; Nwp=-1.753e-04; Xwq=-1.012e-02; 

Ypq=1.753e-04; Npgq=-7.503e-04; Zpr=1.753e-04; Mpr=7.503e-04; 
Xqq=4.982e-05; Xrr=4.982e-05; Z2DS=-4.216e-03; MDS=-2.176e-03; 
XWDS=2 .226e-03; XODS=1.148e-03; XDSDS=-1.429e-03; 

YDR=4 .216e-03; NDR=-2.176e-03; XVDR=-2.226e-03; XRDR=1.148e-03; 
XDRDR=-1.429e-03; KDA=9.674e-05; XDADA=-2 .858e-03; KPHI2=0; 
KPHI4=3.506E-05; 


$long. center of rotation off the body 
Zqaq=—-4.121E-03; Mgagq=-1.770E-03; 
Zwaq=-2.2/0E-02; Mwaq=-9.487E-03; 


Zw2=-5.917E-02; Mw2=-2.474E-02; 
2w3=3.750E-03; Mw3=1.132E-02; 
Z2w4=1.293E-01; Mw4=5.463E-02; 
Zwaw=-5.643E-02; Mwaw=-1.135E-02; 
Zq2=-4.743E-03; Mg2=-1.607E-03; 


Zqaw=-3.712E-02; Mqaw=-1.107E-02; 


$lat center of rotation on the body 
Yvav=-5.643E-02; Nvav=1.135E-02; 
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Yvar=2. 2/0502 - Nvar=-9.487E-03; 
Yr2=-4.743E-03; Nr2=1.607E-03; 


$lat center of rotation off the body 
Yrar=4.121E-03; Nrar=-1.770E-03; 
Yrav=—-3.712E-02; Nravel.i107E-02; 
Yv2Z=-5.917E-02; Nv2=2.474E-02; 
Yv3=-3.750e-3; Nv3S=L.132e-2; 
Yv4=1.293e-1; Nv4=-5.463e-2; 


$Dimensional design for steering autopilot 


livcdiot=Nva~0.5*rho*hL- 4; 

irdot=Nrd*0).5* rno*L* 5; 

mVCot=YVa-0.5"rno* lL. 3; 

Mmidoc=—1rd O.o7 Lno* lL 4;NV—Nv*~0.o* Eno*V*L" 3; 
Ne=Nr*0.5*rho*V*L" 4; 

imu=1V~U- 5 rho VL 2? 

me=yr* 0 .5* rho*V* iL" 3; 

Nea=NDR*0.5* rno~V°2~L° 3; 

ra=YDRA0.5* Biot /V°2* il 2; 

MM=[(m-Yvdot) -Yrdot O;-Nvdot (Iz-Nrdot) 0;0 0 1); 
AA=[Yv (Yr-m*V) O;Nv Nr 0; O01 Oj; 

BB=[Yd;Nd;0]; 

A=inv (MM) *AA; B=inv (MM) *BB;C=[0,0,1] ;D=0; 

{num, den]=ss2tf(A,B,C,D) ;z=roots (num) ;p=roots (den) ; 


% desired closed loop poles for sliding are [-0.4,-0.41,-0.42,0]; 


k=place (A,B, [-0.4,-0.41,0]); 

$ closed loop dynamics matrix 
Ac=A-B*k; 

[m,n]=eig(Ac'); 

s=m(:,3); 


$Roll Controller design 
hx=—32.1/626; 
nRea=KDA*O0.5*rho*V 2*L 3; 
Ar=[0,1;-0.02*88.95*32.2/1x,0]); 
Br=[0;Kd/Ix]; 
br=place (Ar, Br,[-1.4,0]); 
Acr=Ar-Br*kr; 

(m,n]=eig(Acr'); 

s=m(:,2); 


%$ Observer Gain Matrix 
Cr=eye(2,2); 
me—-Diace (Ar ' er’, [(-l.407-1.41)). 
s*Kc=place (Ar',Cr', [-0.60,-0.61]); 


% Roll Observer Design 
Aor=Ar-Kc'*Cr; 
Bor=(Br,kKc' |]: 

Cor=-Cr 

Dor={zeros (2,1),evye(2,2) ] 


end 


8] 





APPENDIX B. MATLAB FILES FOR 21UUV COMPUTER MODEL 
This appendix contains Matlab files used in the 21UUV computer model. All 


files were originally written by Healey and Miguel and modified to meet the needs of 


specific simulations. 
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FuNnCeE1 On 


body _ vel.m 


fdu;dv,dw,dp,dq,dr}]—body vel(u,v,w,p,d,t,pm1, cheta, Us, DR, DA, Exp ice 


SFxh, SE yh, 


global rho 


global Xud 
Yrelie, 

global Xvv 
global 2ZDS 


tl = 

t2 = 

t3 = 

t4 = 

t5 = 

t6 = 

ty = 

t8 = 

t9 = 

t10 = 
t13 = 
t14 = 
t15 = 
t16 = 
t19 = 
t20 = 
t23 = 
t24 = 
t25 = 
t26 = 
t31 = 
t32 = 
Ss. = 
t34 = 
t36 = 
t39 = 
t40 = 
t41 = 
t42 = 
t43 = 
t44 = 
t47 = 
t50 = 
t52 = 
t55 = 
Ea6e— 
t57 = 
t58 = 
t60 = 
tel = 


Fzh, SMxh, SMyh, SMzh, Mxsin, Fzcf,Mycf, Fycf,Mzcf) 


Mepewgoee) 46 LX Lyeiz Se pram; 
Yvd Nvd Zwd Mwd Zqd Mqd Yrd Nrd Xuu Yv Nv Zw Mw Kp Zq Mq 


Zvp Mvp Xvr Xww Ywp Nwp Xwq Ypq Npq Zpr Mpr xXqq <Xrr; 
MDS XWDS XODS XDSDS YDR NDR XVDR XRDR XDRDR KDA XDADA; 


ZnO” Lho-; 
SMyh*t1; 
a 

373; 
t4*t4; 

to* Mad; 
eeu G,; 
SEFzh*Zwd; 
Sin(theta); 
t9*B; 
Wot: 
lS: 
t5*Mwd; 
Shzn-~2qa; 
Zg*2Zg; 
tlo*Wo; 
SFzh*rho; 
Sa 
t24*Zwd; 
EZ32 E25; 
SMyh* rho; 
aes 
t32*Maqd; 
Gol Css; 

NM Ly; 
t31*t4*Mwd; 
m*Zg; 

cos (theta); 
cos (phi) ; 
t41*t42; 
E45*B: 

Me 1; 

tale 32- 
Bm: 

2 eho: 
SMyh7 ESo; 
ES ES. 
t5/7*Mwd; 
GoO6s CoS Se zn: 
Agar seen: 
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C6Sy— Uw; 

tO4w= coo. bs; 
t67 = u*u; 

t68 = XDADA*t67; 
t69 = DA*DA; 

t70 = t68*to69; 
t/4 = v*u?; 

t75 = t74*DR; 
t78 = XDRDR*t67; 
t79 = DR*DR; 

£6 08=8C78*C79; 
EO Se=8t5* tz4; 
t84 = t83*Mwd; 
£86 = C56* E6455Fzh; 


COCw= usr, 
ES SS=8tS6“DR; 
Eo ua; 


t94 = t93*DS; 

£97 @-8£56*t568; 

t98 = SFxh*Xuu; 

t99 = t98*t67; 

PlO2 S=9=2.0* tits tl0-20* t1l4*tis*t16r4 . 0% b2047 em eZ 6— 
a Oeerle iy t 260-4 .°0... 
Beso, Go4ltow)- tls > tso-4.0=ts9"t40-t44— 
oe ect o~t4 /+4,,.0*t50*Mod-to*t52+to0™. .. 
=o <XWps* F64+tE60*t6l1*t/6+E60* E61*XVDR*E/5+ESGO* ESI *tS0+ES6* E6l ~*xXRDR*~tS 9+ 
Gor LOlL* 3. 
XODS* t944+t97*t16*t99; 

£103 "°= AVE7V- 

ClO4 ae Clos re; 

tl07 = Xwaq*w; 

El0s = tl0j-a; 

fib tae 4; 

Eee. t56-cl1li*Mwad; 

t114 = SFxh*Xrr; 

Pas. = yr 

PibhS = tlle cl is; 

Piha Lol 4: 

t120 = Mwd*t47; 

Eva — pp. 

Gl25 = -q"q; 

Glzoe—=Eo*tc; 

els 5e= sl V7 SEzh; 

Gls = tls3* rho; 

eis] = £2*tLo- 

t140 = t43*Wg; 
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4.0*Cll02seesic* ..2; 

€1117-—40e 4 OSes e652- 

2.07 € bis tilda SEyh*Yrd+2.0* tl Were sce 742.07 
Cli2seeecge. cline 0 >ee99*tlils3—4.0*€l0l4*t)25*elios, 


t1141 = t67/*DA; 

t1142 = SFyh*Yvd; 

tae s. = ebla2*iz; 

t1151 = m*SMzh*Nrd; 

tie ee — ede l*té3*Mxsin; 

t1161 = t40*t88*SFyh; 

Elhico) = up. 

PiteZ — 6eu2*p; 

tlte6,—sc105*q; 

taeb2e = 2.07 E1091 *t154*KDA*tli4i*tiiaec- 


EMOCAT logs el 0434220">t1091*tS" KDA... 

Eire lilstp4eOet4O~tss*tlO0GSs-tlis/*tll42*t /69=zeG-tlilel*ce1049— 
AO eel 2 4 ee. 

KDAY Dae 4 i200 te LO9l*tlo6e*Kp*tlleo*tlil43+tl bey ee 28a iS 
Are El bibs 4 Rice. . 

EGO Baga ee Otel Oger 14 5*Kp*tllo9*tlisi1— 

AO tO eo, sselezts .0*t4s4*t569-4.0... 

“CMO aoc tl tes ono t40*t /Ol*Fyct+270=tlicl*tilvscs- 


Glos) = ti0et-tlil*Kp*u; 
t1196 = p*SFyh; 

Elo = ta0-t5e6e*tl43; 
tiZ09 = t40*t568*t5? 
t1214 = NDR*t67; 

El219. = Neeru; 

t1224 = t40*t581; 

pie Oee £4344%255 |; 

tl233 = Nea yp; 

t1238 = Nwp*w; 

ti243 = Clo14*c725*SFyn; 
tlZ438 = =tll95*t1196*t1048+t1195*e1196*t1052- 


4.0" t2lOGet x Shy hne-e ly 420%... 

t1205*c63se*tElise-2. 0*~E12094*t636*t1182-8 .0*t564*t3671z—- 

2.07 E11 2372 4A Syne. 

Yrd- 

2 O* C1200 4c et oe recryn* Yrd+4.0*t1224*tos2* Sbynthyecrt2 O4e12097 eeso7. rt ly 
Soe 
=A Or GlZ30%toee to64-2.0r ties tlZ3s370q-SEyh*Yrd- 
2707 E1209 £1235 c Poon Yrd-2 507. 
ti243*c1053—4. Os tl 224 7b 562" tills 342,07tiz4es*ctl04d9; 


dp = (t10804+t11374t11924+t1248) *t779/2; 
tl257 = Mwd*SFxh*Xud; 

t1263 = SMyh*Mwd*m; 

t1273 = p*SMyh; 

ElLZo)7 = C402 blz 57-¢ 

t1301 = t612*Mwd*m; 

t1l306 = — 


t781*t143*Zw*u*w* SMyh*t12574+2. 0*t789*t154*Zw*t63*t1263+2.0*t789*... 
t166*Zvp*t185*t1263-t781*t57*Zvp*v*t1273*t1257- 
t781*t57*Zq*u*t413*t12574+2.0*... 
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mes el6e7Zq7 t95*tlZos— 
EVolmuics2Ds tog = Do sMyh*tlZo7+Z.0"t1 69 Clos mio Eaoo .... 
blzi6s=2q0se4 0721 ~SMyh* 61297+4-0*t163*SMyh*el30l— 
2.0" Gt6S*e2*t167-to27: 

t1308 = t612*SExh; 

t1311 = t154*Zwd; 

tl3si2 = €789-tl311; 

GlovG = E7ol-to? *Zwa-seszh, 

Epistle = rhe ts; 

Elg23 = tCilsie*Srzh- 

Piss 0 = DS*t47*2Gq-; 

Pilsa9.— 820*to34*Mvyet— 
Qn O07 tl s0e=t441*ble4+2704t13i2*t793+tl316*teo0—4. 0... 
£1318*t307*t308*t184+8 .0*t438*t304-4. 0*t1323*t372*t184- 
AO tilszs7ta4deg*tils4—4.0*... 
Miwo segs bos tls s0-4.0*ctisle*t221*t222*cle4—-420- cise -t239*t239-t184; 

t1341 = t184*t260; 

t1345 = DR*t47*Zg; 

Elbsd6 = ti3s4*c150;> 

Gls o0) = .e6lott47 : 

t1366 = t444*t47; 

t1372 = 4.0*t943*t1341-4.0*t444*t390*t88*t1345-4.0*t943*t1348- 
AO =e S23 22% 
mo 967t184—4.0*t444*t393*t93*b13530t4. 0*t1355*SMyh*Mvp*tilss— 
4.0*t444*t149*t1348... 
peO Eada t 259 C1 34ir4.02tl1355* SMyh*Ma*t93+4.0* tlsee>smyh Mw tos— 
eo - te rs0s-t404... 
~tE 164; 

t1376 = t47*p; 

Clsiie— x17; 

t1381 = t43*Wg*SMyh; 

t1384 = m*rho; 

£1385 = t13847t340; 

blS30]— 6437 B- SsMyvi- 

tilsgo"— 4707 cls66"sMvn*MDs*t405+8.0*t13/o2 e137 7— 
oed-t1376*to764+4 .0*f1381*... 
Ps Ol—-o.07t10*t184+4 .0*t1355*xud*Zq*tl3— 
fo *ELS90*t1L30l+4.0*t26* F1l007+2.0*c1390... 
melo —-2.O* tlsi2*tiote—4,0*E1318*t393*t/4*tlse45—s.0*-eZ2ll*tile4- 

t1404 = t781*t143*Zwd*SFxh; 

t1409 = m*tl; 

t1411 = t1409*t166*SFxh; 

t1418 e1e097 154 *Srxh. 

t1420 = Xud*p; 

t1428 = t1404*t7874+2.0*t1312*t809-2.0*t1381*t12974+t1316*t813- 
eo La Le 
molls+Z.0*C4208b1273* 6129714. 0*tls4*tlZ2i*t33— 
ZO C1418*e80/+4.0*€1385*b1420* ce76..% 
merc eles tl 2o*ts9-2,0* 240" t1Il25*sMyh*tlZ9o7 ; 

t1436 = t32*Zwd; 

t1437 EI39*tl436; 

t1441 €/39*€1436*SFxh; 

t1445 = t789*t166*Zwd; 

tl45l = 4.0*t47*u*q*t39-2.0*t753*t413*t1297- 
SO t4aZ (Apr esore 0 ct l4387*thoo.... 
+2.0*t1437*t8584+2.0*t1441*t863-4. 0*t26*t8994+2.0*t1445*t918- 
ZeO7 esto 1S 4204... 
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tlsso*blaz0*els7 7=2207e147 1472800, 
elas) = t/eo- tisha sen. 
t1467 = -2.0*t1418*t787- 
AoO* ELS 84*E24*E80942.0* £1457*€8S74+2.0*cI44al- ces. 
270=cl44aeese2t2.0*t1441*t8744+2 .0*t1445*t384742. 0* €14s 68447270 eo ee 
S5022.0* 2. 
Plas ces0r2 07 tl4s57*£836; 
t1l482 = 2.0*t1457*t841-2. 0*t1457*t894-2.0*t1445*t890- 
4.07 €257 ©24*t365"* 3... 
E451—2- 0.1445 "6325-2 .0*tl3siz*tss2— 
220*ttsiZ-tols+el404*e00/+4.0*t26*t469=—4.0*. 
CZ6°t4 7642 o0* ci44ai*ct377+4.0*t26*ts7l; 
dge=] (CIls0G6+tlss9+t1l3/2+C13997 F-14235 cae Lila67 + ble) tee. 
t1486 = t166*Nvd; 
il oS FE Sela ca*Nvd: 
t1494 = ¢£568*t1486; 
tlao? = ©5686-6154 *Nvad; 
Pls0S > = E767 ElLS4*Yvd- 
clsSo04)= ix*SMzh; 
meio = t76),>tl66* Yvd; 
£1517 = -t568*t1486*m*t608-2. 0*t1491*t59342 .0*t1491*t590- 
t1494*t628-t1497... | 
*t666-t1494*t746- 
ElAS 7 ~toeocte .0*t1491*€7 3517150387 b1504* F1124*Vtt/67~b1058*t1504 2. 
~elcocmGttel. bo, ect oue- bl alo 7: 
E157) >= CS Nvae 
EiosZ Eoad sly; 
tilose = t4*Nwp* t571; 
pilSsa0.—seloOs Cl504=t1l214*DR— 
CoCo oieanieerowc melo ot l504*til238*p-toos*. .. 
EMSZ tee Slee, O- cleo E585—-4.0*t488*telo+4 .0*t48s*t1532-270-tl4o ibe 40- 
tC 149A 
C5) 94220 tb 14 Ole 5e742.0*t1230*t1538; 
tisas = t4*Ne*tss; 
t1547 = t24*NDR*t632; 
ElsS0 C32 “Npq* t647; 
els o9 = EZ4*Nv* C7 43 
tlo6cl = E4ase*t5ssl; 
t1566 = 
2207 el2s0eeloaot4 6 t434*t6l0+Z.0*t1230" clee 420 e230 El 5502 0]. 
*t763*t764*t61044.0*t434*t750-4.0*t434*t1532- 
220°C LO4A2= 66447 t492+2.0*t12307 -3. 
tl559-2. 0O* Elo6l *el538+2.0*t763* Yvd*t648*t647- 
tlse2 = —2.0*€1561*t1547-2.0* €1561*t1550-2.0*t1491*t727-— 
4.0*m*t6o48*ct64/ 2.4 
=—2.0~=tlS6t~tls3oo—2,0 clool-t154342.07t1491*t730— 
2.07 C149" t645+202Graol* e754 oe. 
-2.0" Goel t4"*Nvde syn. tos7—-2.U0*tl491“teza- 
dr = £779* (€15174+t1540+t1566+t1582) ; 
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control_design.m 


% This program is computed in feet/sec/lbf units 

$ the depth controller for a submersible vehicle in forward 
$ motion is designed and computed based on SMC methods. 

%$ Nondimensionalized version of Hydrocoefficients are used 
$ and reconstituted into dimensional form. here rho=1.94;L=20; 
%$ Time is in seconds. V is in feet / sec. 

clear 

V=6; 

t0=0; 

tf=300; 

rho=1.94;L=20.5; 

% 

%$ Vehicle nondimensional parameters for the DSRV 

$ diving response 

% 

PeeEnisce sets Up Che diving sme design 

% 

* open leop system 

$ DIVE CONTROL 


m=88.9518;%slugs 
Tyy=2632.47;% dimensional already 


Mea ite) 1C-J0S70.5* rho V~L°4; 
Madot——7/.504e-0470. S*rho*L’ 5; 
Mw—6. /46e-03*0.5*rho*V-L 3; 
Mwdot=-1.753e-04*0.5*rho*L%*4; 
Md=-2.176e-03*0. 5*rho*V%2*L‘%3; 
MeEh=0.02*m*32.2; 
i——Zoofe-05- 0.5 Ene Vl Ss; 
Zq00t=—-1./53e-04*0.5*rho*L*4; 
Zw=-7.406e-03*0. 5*rho*V*L*%2; 
Zudot——1.,04le-02*0.5* rho*L’ 3; 
Zd=-4.216e-03*0.5* rho*V*2*L*%2; 


PODetaines2s l,/v — 20/74 = 5 seconds) 


MM—| (m—-Zwdot), —2qdot, 0, 0; -Mwdot, (Lyy-Mqdot), 0, 0,0, 07 1,0;0,0,6,1), 
mre — {2W, (Zqtm*V),0,0;Mw,Mq,Mth, 0;0,1,0,0;1,0,-—V, 0}; 

Pee {(20;Ma;0;0)]- 

A=inv (MM) *AA; B=inv (MM) *BB;C=[0,0,0,1] ; D=0; 

[num, den] =ss2tf(A,B,C,D) ;z=roots (num) ;p=roots (den) ; 


sme oesi red closed loop poles for sliding vercm.0.47 Ur, 0.4270); 
k=place (A,B, [-0.4,-0.41,-0.42,0]); 

%$ closed loop dynamics matrix 

Ac=A-B*k; 

[m,n]=eig(Ac'); 


s=m(:,4); 


% Critical Speed for Input reversals 
% 


oF 


Uc=|[— bi) CA(2 


% 
% 
3 
% 
s 


$Steering Contro 
Iz=ly; 


% 
Xud=-1.667e-04; 
Z2wd=-1.041le-02; 
Yrd=1.753e-04; 
Yv=-7.406e-03; 


$increase linear 
Kp=-2.423e-06; 
Zq=-2.655e-03; 
Xvv=4.073e-03; 


wirce—-UZ; 
O41le-02; N 
Ypq=1.753e-04; 
Xqq=4.982e-05; 
XWDS=2.226e-03; 
YDR=4.216e-03; 
ADRDR=-1.429e-03 


AVr=l. 
Ywp=1. 


KPHI2=0; 
KPHI4=3.506E-05; 


$long. center of 
Zqaq=-4.121E-03; 
Zwaq=-2.270E-02; 
Zw2=-5.917E-02; 
Zw3=3./50E-03; 
2w4—1, 2935 -c1- 


Zwaw=-5.643E-02; 
Zq2=-4.743E-03; 
Zqaw=-3.712E-02; 


p3)*V"2)/(A(2 es) Biz (2, 1) ) “eS 


Simulation of closed loop response 


1 design 


Nvd=1.753e-04; 
Zqd=-1.753e-04; 
Xuu=-8.348e-04; 
Z2w=-7.406e-03; 


Yvd=-1.04le-02; 
Mwd=-1.753e-04; 
Nrd=-7.504e-04; 
Nv=-6.746e-03; 


Mqd=-7.504e-04; 
Mw=6.746e-03; 
roll damping *10 


Mg=-1.477e-03; Yr=2.655e-03; 
Zvp=-1.041le-02; Mvp=-1.753e-04; 


Nr=-1.477e-03; 


Xww=4.073e-03; 

wp=-1.753e-04; Xwq=-1.012e-02; 

Npgq=-7.503e-04; Zpr=1.753e-04; Mpr=7.503e-04; 
Xrr=4.982e-05; ZDS=; MDS=-2.176e-03; 
XQDS=1.148e-03; XDSDS=-1.429e-03; 
NDR=-2.176e-03; XVDR=-2.226e-03; 
KDA=9.674e-05; XDADA=-2.858e-03; 


XRDR=1.148e-03; 


e 
f 


rotation off the body 
Mgagq=-1.770E-03; 
Mwaq=-9.487E-03; 
Mw2=-2.474E-02; 
Mw3=1.132E-02; 
Mw4=5.463E-02; 


Mwaw=-1.135E-02; 
Ma2Z=—-1.,607E-03; 
Mqaw=-1.107E-02; 


$lat center of rotation on the body 


Yvav=-5.643E-02; 
Yvar=2.270E-02; 
Yr2=-4.743E-03; 


Nvav=1.135E-02; 
Nvar=-9.487E-03; 
Nr2=1.607E-03; 


$lat center of rotation off the body 


Yrar=4.121E-03; 
Yrav=-3.712E-02; 
Yv2=-5.917E-02; 
Yv3=-3.750e-3; 
Yv4=1.293e-1; 

% Dimensional 


Nrar=-1.770E-03; 
Nrav=1.107E-02; 
Nv2=2.474E-02; 

Nv3=1.132e-2; 

Nv4=-5.463e-2; 

design for steering autopilot 
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Nvagot=Nvd"*0.5* rho*L” 4; 
Nrdadot=Nrde 05> choco); 
YvVaGt=rva'O.5* rhe 3; 
YEact=yra~0.5* rho L 4 


WV=NV*O0p> {Eno Vv" bos: 
Neaiinr 0. o*tho*V* LL" 4; 
Weve Oe thot Vv" L 2% 
mee YEO. oc Enoe V* lS; 
Na=NEK= Ol o2 rno*V- 2° 3; 
Mo=—YDR= 0a. cho Vv 27 1, 2)- 


MM=[(m-Yvdot) -Yrdot O;-Nvdot (Iz-Nrdot) 0;0 0 1]; 
AA=[Yv (Yr-m*V) O;Nv Nr 0; O01 Oj]; 

BB=| Ya; Na; 0]); 

A=inv (MM) *AA; B=inv (MM) *BB;C=[0, 0,1] ; D=0; 

[num, den] =ss2tf (A,B,C,D) ;z=roots (num) ;p=roots (den) ; 


S desired closed loop poles for sliding are [-0.4,-0.417-0.42, 0]; 


k—piace (A,B, (-0.4,-0741,0)); 

% closed loop dynamics matrix 
Ac=A-B*k; 

[m,n]=eig(Ac'); 

s=m(:,3); 


$ Roll Controller DESIGN 

ae x 

ix—32 5/626; hpdot—0.0;Kp=—2.423e-06*0.5*rho*Vv"*L 4; 
Rphi——-m-o2.2" 0.102; 

$zg=0.02' 

Ka=0.5*V*%2* rho*L*3*9.67/4e-05; 
A=(0,1;Kp/Ix, Kphi/Ix] ;B=[0;Ka/Ix]; 
pdes=[(-1.5,0]; 

=place(A,B,pdes); % k=[-0.0835 -0.3002] 
Ac=A-B*k; 

[m,n]=eig(Ac'); 

pai s,2); 9 Ss — 0,0) 


a2 


euler2body6d.m 


function output=éulerZbody6éd (Dx, Dy, Dz, Dphi, Dtheta, Dosi, phi, theta, psi) 


% euler to body velocities transformation - generated automaticly with 
maple - inertial.ms 


tl = jeosi(psi); 
t2 = cos(theta); 
t5 = sin(psi); 


t8 = sin(theta); 

Ubedye— El e2*Dx+to*t2*Dy—-te- bz; 

oe. — Olt tenis 

Elie — sin (phd) ; 

tiZe=etro- tli: 

Ei = Dx tos 

ElSe> cos (phi); 

Ee ie Dy et 57 

ioe — ey C1; 

Veody —st LOI Z-ela Se lSt tl ceZ eo Ee lot el eZ) pz; 
U23e=—sto tls; . 

Wbhody = t10*t23+t14*t1l1l4ttl17*t23-t19*tll+tl5*t2*Dz; 


Phody = Dphi-sin(theta) *Dpsi; 
Eee —= cos (pha); 

E> San (Dad); 

t6 = cos (theta); 

Obody |] t3*Dthetatts* to-Dpsi; 
Rbooedy = —~ts*Dthetat+es=r6" Dpsi,; 


output=[Ubody, Vbody, Whody, Phody, Qbody, Rbody] ; 
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faultsgen.m 
function [sys, x0] = faultsgen(t,X,in, flag,mode0, FaultM, Faultfile) 
global faults findex; 
if abs(flag) == 2, %$ return of discrete state 


event hit=faults(findex ,1)==t; 
eee Vee sity, 


Sys=X; 
sys (faults (findex,2) )= faults(findex ,3) 
faults 
findex=findext+l 
else 
sys=X; 
end 
elseif abs(flag) == 3 % Return systems output 
% (vector of signals that control actuators function mode) 
Sys=X; 
elseif abs(flag) == 4 % return the next time in which will occur some 
fault 
sys=faults(findex ,1); 
elseif flag == 0, 
x0=mode0; 


numOutputs = length(mode(0) ; % dynamic number of outputs 
numsStates=numOutputs ; 


Le Nacguaq 2, 
FaultM 
[faults,I]=sort(FaultM) ; 
faults (:,2)=FauleM( bt. 2) 72); 
faules (2,3) =raulemil(:73)),3) > 
faults (length(faults),:)=[10000,10,10]);%insert add line to matrix 
$ so that findex not exceed matrix size 
elseif nargin==7, 
F = csvread(Faultfile); 
[faults,I]=sort(F); 
Faults (2 Poyr—eE (i (eee), 2):> 
Eau bes (2s) =P (b(:, Lis) 
faults (length (faults) ;,: )}=[10000, 16, 10]; $ insert an add line to 
matrix 
%$ so that findex not exceed matrix size 
else 
error('wrong number of parameter to faultsgen s-function') 
end 


findex=1; 


sys=[0,numStates,numOutputs,0,0,0]; % 0 continuous states, n 
discrete, n outputs, 0 inputs 
else 

sys=[]; 

f=flag; 
end 


10] 


inertial.m 


EUNCET On eine remeu epee e) = 
tierlialile,in, flag, Xinw@, yini, Zinl, pwr, ENnetalni, psa imam 


Scuopdiielime vel body DRvecpsiyclneta econ? setheta spc) ena: 


i 


dP oP oP cP MN oP oP oP OP 


f abs(flag) == 1, $ Return state rates 


T=zeros (3,3); iner=zeros (1,6); tin) vel pody=zeros (3s, )7; 
p=zeros(1,1); q=zeros(1,1); r=zeros(1,1); 
phi=zeros(1,1); theta=zeros(1,1); psi=zeros(1,1); 
cpsi=zeros(1,1); spsi=zeros(1,1); ctheta=zeros(1,1); 


theta=zeros(1,1); cphi=zeros(1,1); sphi=zeros(1,1); 


DR=zeros (3,1); 

dphi=zeros(1,1); dtheta=zeros(1,1); dpsi=zeros(1,1); 
ineweis bodyein (1 a); 

anime Hm ocly —IN4) 2167), 


u=in (1) ;v=in(2) ;w=in(3); 
p=in(4) ;q=in(5) ;r=in(6); 
Phisn (a 

theta=X (5); 

psi=X(6); 


[dx,dy,dz,dphi, dtheta,dpsi]=inertial eq(u,v,w,p,q,r,phi, theta,psi); 
inert out=(dx,dy,dz,dphi, dtheta,dpsi] ;% +WAVE* Swave; 


$ inert _out2=euler2body6([u,v,w,p,q, r] +WAVE*Swave) ; 
% inert out=[inert outl,inert out2]; 


elseif abs(flag) == 3, 


Peewee =X; 


elseif flag == 0, 


Ss 


$ 


$Return initial conditions 
inert out = [(6,0,6,6,0,0]; % 6 continuous states (inertial 
tate, . 
$ 0 discrete, 
% 6 outputs (state), 
%$ 6 inputs (body rates) 
Mini yin, Zit, phiiinl, Chetan, pot Mt ||; 


O= [ 
xO = (0,0, 070307 o 


else 


inert_out = []; 
f=flag; 


end 
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inertial_eq.m 


fune cron 
[dx,dy,dz,dphi, dtheta, dpsi]=inertial eq(u,v,w,p,qg,r,phi, theta, psi) 


%$ euler angles transformation ~- generated automaticly with maple - 
inertial.ms 

Cpsi = cos (psi); 

spsi = sin(psi); 

ctheta = cos(theta); 

stheta = sin(theta); 

cphi = cos(phi); 

spha = sin(phi); 

Ge =) Vv" Cosi, 

t4 = stheta*sphi; 

t6 = v*spsi; 

t8 = w*cpsi; 

t9 = stheta*cphi; 

tll = w*spsi; 

Gear — ‘Cpsil-Geneta Utts* e4-c6~ cohit+te eyttli*sphi; 

dy — spsil* ctheta*ut+t6* €476S*cponitrell*to-cso* sph; 


dz = -stheta*utctheta*sphi*vtctheta*cphi*w; 
t28 = 1/ctheta; 
adphi = (p* cthetatrt4* qteo*r) *t26; 


dtheta = cphi*q-sphi*r; 
dpsi = {(sphi”grcpni*r)*e23; 
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maxllike.m 
$ this program was originally written by R. Christi, NPS 


$likelihood ratio test 
$t=(0-0- 12100); 
$f=rand(1,1001) ; 
$m=mean(f);s=std(f£) ;f=f-m; 


Seon i=l]: lengen(t)., 

% if i>400,f£(i)=f(i)+1l*s;end; %3 sigma sudden jump in residual 
$ end; 

% 

$figurae(l),clii,plot (f£) 


% Sjk=d*2/c 
{ephi,g] —c2d (Aor, Bor (:, 1) 9021) ;x=zeros (2, (length(t)48))-; 
for i=l:length(t); x(:,itl)=phi*x(:,1)t+g;end; 
TNO—x(2,5 ) > 
$r=zeros(1l, length(t) );rho=r;rho(1)=0; 
$ generate residual signal excited by wn only 
$for 1=2:length(t), 
Seta, SO0G* rT (1-1) =0.81387* 5 (i-2)40 7090S e Ria 0 09057 £(i1—-1) 7; 
% Magid i  oOUG* rho (1 —l) 02 oled= tho (1-2) 
$ xr(i)=0.9*r(i-1)+0.1*f£(i-1) ;%filtered signal noise t+fault 
$ rho(i)=0.9*rho(i-1)+0.1*1;%filtered signal no noise + anticipated 
unit fault 
$end; 


Sl GuULre tw), CLE 
SEI gGuBeN 1) -clf, plot (t, 1) 


[phi, g]=c2d (Aor, Bor(:,1),0.1) ;x=zeros (2, (length(t)+1)); 
fotea—l-tengen(G), 0% (:,itl)=phi*x(:,1)+gq; end; 
rho=x (2,:); 
M=50;N=M-1; 
d=zeros (length (t) ,M) ;l=d;c=zeros(1,M); 
for k=M:length(t), 
£ore j=l: M; 
@(q)=Ehol (ke +1): k)*rho( (k-j41):k) "> sAutoCorrel unit fault 
G( kya) ene —g +) ik) roll obs) res ((k-j+1):k,2) 7 sereossecor 
resid with unit 
1 (k,j})=0.5*d(k,3)*2/(c(j))? % increase shows fault level 
end; 
L(k)=2*max (1(k,:)) ;fhat(k)=d(k,M)/c(M) ; 
g(k)=max (roll veobs res(k=N:k).°2-(roll obs rés (K=N:k)=that(ke 
Ne) = 2yaz 
end; 


EVGure(Z\necl £, 
plet(t,r,'r',t,;that; m6, be. agra 


Lrigure(s) clit, 
PlOE(t, 6), Grd 
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model.m 


function [sys, x0] = 
modeistt, x, IN, flag, Wind, Vln wind ,pinil, Gini, rini, amo) 


global rho m 2-Wo By2Zg [x Ly 1Z xt Dian; 
global log cross; 


if abs(flag) == ; % Returns state derivative 


mneritns Cate mw Len—o, 
PiSRCEALeo Late iN Cieanere state mLenyr, 
PINSS@MDS-ING@imese,seatce len tl: ianert state len +4); 


Prep nN-IN(aneue, stakeglen +5); SPEC pele pare Vommeaos ) 
u=x (1); Maer (2) W=x (3); 
p=x (4); q=x (5); r=x (6); 


phi=INERCIAL STATE (4); 
Piet INERCIAL | STATE(S); 
pZ2=INi 3); 

delta s=FINS CMDS(1); 
delitayr=Fins Temps (2); 
delta a=FINS CMDS(3); 


Prop wf=0.8; 

PRopealta-O73; 05% 2 22%7eA seree: |! 

Prop, D=2*8. 05/12; 

Eap-EhO-Enbop In: abs (Prop en) Prop D4 *0).4— 
Erepealfa “Prop mn - Prop DD 3* rho u, 


$Fxp=16; 
Mxp=0; 


SMxsin=1; Sdraglong=1; Sdraglat=1; 
$SFxh=0; SFyh=0; SFzh=0; SMxh=0; SMyh=0; SMzh=0; 

&$ SFxh=0;SFyh=0;SFzh=0; SMxh=1; SMyh=1; SMzh=0; 
SFxh=1.0;SFyh=1.0;SFzh=1.0;SMxh=1.0;SMyh=1.0;SMzh=1.0; 


% crossflow evaluation - 


fizsin, Longit,rzci,Mycr, lat, Fyct,Mzcrt)]=crossflow(u,v,w,p,d, ©, saraglong, 
sdraglat, SMxsin;1i, Diam,1); 


tegmetoss (lenge (leg cross) !,.)—lt,;Mxsin,longit,hzct, Myer, lace, Pyer, 
MZEr ||: 


% 6DEF EOM - generated automaticly with maple - model.ms 
% 

(du, dv, dw, dp, dq, dr] =body_ vel(u,v,w,p,q,r,phi, theta,delta s,delta r,d 
ela a, Exp, Map, ohxh,ohyn, SE zh, sMxh,oMyh,sMzn, Mxsin, Fzc£, Mycf, Fycf, 
Mzcf 5 
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Ldugdvdw,dp,dc,ar |= 
bodym(u,v,w,p,q,r,phi, theta,delta s,delta r,delta a,Fxp,Mxp,SFxh,SFyh,S 
Ean, oMxh,oMyh,SMzh, Mxsin, “F ZG gy C Meet ici) | 


Sys (du,dav,dw,dp,dq, dr]; 


elseif abs(flag) == 3, 


$WAVE CX=1; WAVE _CY=0.0; 
$ water current in ft/sec, H=water depth 
H=30; | 
WAVE _CX=0.0; WAVE CY=0; 
WAVE AMP=amp; WAVE _T=5; 
WAVE K=(2*pi)*2/ (WAVE T*2*32); $(2*pi)*2/ (WAVE _T*2*32) ; 
WAVE V=sqrt (32.2*H) ; $= w/k = 2*pi/WAVE_T/k = WAVE_1T*32/(2*pi) 


WAVE=wavevell (t,IN(1),IN(3),WAVE_V,WAVE_AMP,WAVE_T,WAVE CX,WAVE CY,H 
a 
% WAVE(7) = pressure 
%&Swave=0; 

xwave= WAVE(1:6)'; 


sys 
= (xteuler2body6d (Xwave(1),Xwave(2),Xwave(3),Xwave(4),Xwave (5) ,Xwave (6), 
ENA ENS) INI 6)) *); 


%$ sys=[sys;WAVE(7)]; 
sys=(sys;x(1:6) ;WAVE(7)]; 


elseif flag == 0, 


pS Se — LO, OG.) , 11,0, 0]; %$ 6 continuous states , 0 discrete, 
i Seuepues, | anputs 
SVse= We, 0,13,11,0, 0]; % 6 continuous states , 0 discrete, 13 


outputs, 11 inputs 
log cross=[]; 


$Return initial conditions 
SOs ehuna Vind, Wind, pinw,cini, Fini | > 
% §xO0==iie,0,0, 0,0,0]: 


else 
sys = [J]? 
f=flag; 
end 
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predict.m 


finer one stole — prediet( delta) 

* Matlab Script to funmetiion as a BEigqhth—Order Digital Filter for 
Predicting Future Seaway Elevation Response 

Tracks and matches Pierson Moskowitz/Pressure Profile 
Spectrum and predicts responses one full wave length 


Variables 
h = Fignificant wave height 
Vows bequeney 
S = Defines Pierson Moskowitz expression for fully developed 


t = Time vector 

zeta = Damping ratio 

T = Period interval 

A,B = Continuous plant model 

Phi,Gamma = Discrete plant model 

K = Filter Gains 

error= 

x2 = Estimate of state vector 

Y = System's output 
rho = Cross-correlation coefficient 
delta is the number of time steps, (i.e. Phi is delta 


0P oP CP OP CP CP OP CP CP CP CP NM cP CP CP CP CP CP OP co 


minus one) 


h=3; 
W—(O0.5:0.05:3)>dw—0. 05; 
(1,ms]=size (w) ; 
S=w; 
for i=l:ms 
Si(i)=8. le=3*3222727(wihi) 5) *exp(=s3. 56/h*2/ (wrt) ~4 ) V5 
end; 
Wo-lO0.520.0525)¢ 


Banbedelbelsol, 146.52,127.62, |l2.06, 100798, 91.21,33-01,76.02;,70,64. 73 2 
Ur G7 OIG, 926257406007 49.605),45.00,40.49,58.14;7 59. 96,535.95, 52.05,50.29 


oo, 2) .1,25.00,24.5,25.05,21,04,20.11, 19 eG, |s.61, 1-15, \eree,460.07,1 
Bo O, . . s 

Po, Los IZ louzo nie. tle lo, nos, bilo, 10.6970). 26 79. Bo no. 4a, 96 ll Oa 
Hepes. 

Bee ie el Og 0, 1 9, OO, WOO Oo |; 


% Pressue Measurement Simulation 


lambda=lambda.*3.28; 
=e— (| 0:0.1:200); 
Y=zeros(1,length(t) ); 
for i=1l:length (ws) ; 
phi=rand(1,length(ws) ) ;phi=phi-mean(phi) ; 
y(i,:)=(cosh(6*pi/lambda(i))/cosh(40*pi/lambda(i)))... 
SSO EV Sa) ez caw)) “cos (ws (i) *ttoha (i) *pi*2) 7 
end; 
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for j=l:length (ws) ; 
VSL Tyg: ) 7 
end; 


$ Defines Coefficients of transfer function of Eighth-Order Filter 


T=0.1;w0=1.28; zeta=0.4; 

n=[(1/w0, 0} ;d=[ (1/w0) *2,2*zeta/w0,1]; % Defines Fourth-Order 
Expression 

num=conv (n,n) ;num4=conv (num, num) ; 

den=conv (d,d) ;den4=conv (den, den) ; $ Defines transfer function 


%$ Defines innovater gains for subject filter 


{[Al,Bl,C1,Dl1]=tf2ss (num4,den4) ; 

{Phi,Gamma]=c2d(Al,B1,T); 

K=dlla@r({Pha',Ci*, evye(s,8)*10,.1); eigl=abs (eig(Phi—kKe- cl) 
x2=zeros (8,length(t)) ;x4=x2;P2=zeros(1,length(t) ) ; P6=P2; 


% For loop for closed loop filter 


for i=l:length(t) 
MoUs atl) =( Pho) *x2(:,1,)tK'*(Y (1) —Cl*x2(7 | 
xOO (3, i+1)=(Pha“delta) *x2(;,171) ; 
P21.) SGiee2( =, 1.) 
RoW —Ccl*x60 (2,1) > 
end; 


for i=l: (length(t)-delta); 
error60 (i)=(P60(i)-Y(itdelta) ) ;Y60(i)=Y(itdelta) ; 


end; 


$ Calculates cross-correlation coefficient between P-M spectrum and 
filter 


rho=(Y60*P60 (1: (2001-delta) )'/(std(Y60) *std(P60) ))/(2001-delta) ; 
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Start_up.m 


SfunctioOn ouc—start upi() 

global log cross ctr_sig esignals edetection N com; 

global kin to Unite kit Comunit Kslugs™eo unrzt kibme to unite 
Iai be Teo unit; 

global klbm_ d ft3 to unit kslugim feZyeonuRLe 7 


global rho m1 Wg B Zg Ix Iy Iz xt Diam; 


global Xud Yvd Nvd Zwd Mwd Zqd Mqd Yrd Nrd Xuu Yv Nv Zw Mw Kp 2q Mq 


Yr Nr; 
global Xvv Zvp Mvp Xvr Xww Ywp Nwp Xwq Ypq Npgq Zpr Mpr Xqq Xrr; 


global ZDS MDS XWDS XQDS XDSDS YDR NDR XVDR XRDR XDRDR KDA XDADA; 


global KPHI2 KPHI4; 


long. center of rotation off/on the body 
global Zqaq Mqaq Zwaq Mwaq Zw2 Mw2 Zw3 Mw3 2w4 Mw4; 
global Zwaw Mwaw Zq2 Mq2 Zqaw Mqaw 


$lat center of rotation on/off the body 
global Yvav Nvav Yvar Nvar Yr2 Nr2; 
global Yrar Nrar Yrav Nrav Yv2 Nv2 Yv3 Nv3 Yv4 Nv4; 


* TO use SI 


=) Kingtogunit=.0254; sco.) 
& kft. EO —unit=. 3048; = (to om) 
% kslugs_ toounit=147 2750, % (to Kg) 
% klbm to unit= =0.4536; $(to Kg) 
$ klbf to unit=4.4482; $(to N) 
% TO LU, sec,silugs, br 
In toueunit-1/12; S(to srt) 
kit to unit=1; % (tO. Lt) 
kslugs_ own tie= i, (to slugs) 
klbm_to _unit= 0).4536/14.5939; (to slugs) 
klbf_ ©, ain il steeylbt) 
elo use other detine. Kin tO Unit, ke emie mun? temo Ss sto mlinine, 
% kKibmetocunit,  kibE sto unit 
kibm da ftes sto untt = "kibmitenunit, (kntetomunit 3), 
Kslug m f£eZeto unit — kslugs__ to _unit*kft_ Pomun? CZ. 
% initial values in 
rho=62.41*klbm d ft3 to unit; Sebo, ft 3) 
m=88.9518*kslugs to unit; $ (slugs) (lslugs = 14.5939Kg , 
lslug=32.171bm) 
[=2400> Kin tosunet; % (in) 
Wo=236R C355" KEDE Tce unt; ® (lbf) (1N=4.4482 lbf) 
Bac0 Pio ote Wl te tOmuniae, % (lbf) 
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ban 


A0-2 Oar Eeeo luni t ; %. (in) 

Pas 327020 ks ugummerea uo unl eE, % (stlug-f£e Z) 
Ty=2632.47*kslug m ft2 to _unit; S. (Ss ug= fe yz) 
12=2632.4/*kslug m ft20to unit; 4s. (slug Le 2) 


RE=ielS. 9979 kin. Comunit—l; 
Diam=20.94*kin to unit; 
Spakbr—|Lo,m,1,N0,B, 20, LX, ly,Lz, x6) 


coeficients 
Yvd=-1.04le-02; 
Mwd=-1.753e-04; 
Nrd=-7.504e-04; 
Nv=-6.746e-03; 


$Nondimensional 
Xud=-1.667e-04; 
Zwd=-1.04le-02; 
Yrd=1.753e-04; 
Yv=-7.406e-03; 


Nvd=1.753e-04; 
Zqd—- leg ase-04; 
Xuu=-8.348e-04; 
Zw=-7.406e-03; 


$increase linear roll damping *10 
Kp=-2.423e-06; 
Zq=-2.655e-03; 
Xvv=4.073e-03; 


Mq=-1.477e-03; Yr=2.655¢e-—03;, 
Zvp=-1.041le-02; Mvp=-1.753e-04; 


Xvr=1.012e-02; Xww=4.073e-03; 

Ywp=1.041le-02; Nwp=-1.753e-04; Xwq=-1.012e-02; 
Ypq=1.753e-04; Npq=-7.503e-04; Zpr=1.753e-04; 
Xqq=4.982e-05; Xrr=4.982e-05; ZDS=-4.216e-03; 
XWDS=2.226e-03; XODS=1.148e-03; XDSDS=-1.429e-03; 
YDR=4.216e-03; NDR=-2.176e-03; XVDR=-2.226e-03; 
XDRDR=-1.429e-03; KDA=9.674e-05; XDADA=-2.858e-03; 


KPHI2=0; 
KPHI4=3.506E-05; 


center of rotation off the body 
Mgaq=-1.770E-03; 
Mwagq=-9.487E-03; 
Mw2=-2.474E-02; 
Mw3=1.. 132E-02; 
Mw4=5.463E-02; 


$long. 
Zqaq=-4.121E-03; 
Zwaq=-2.270E-02; 
Z2w2=-5.917E-02; 
Zw3=3.750E-03; 
Z2w4=1.293E-01; 


Zwaw=-5. 643E-02; 
Zq2=-4.743E-03; 
2Gaw=—-5.47 12-02; 


Mwaw=-1.135E-02; 
MaZ=—1. 6078-63; 
Mqaw=-1.107E-02; 


lat center of rotation on the body 


Mqd=-7.504e-04; 


Mw=6.746e-03; 


Nr=-1.477e-03; 


Mpr=7.503e-04; 
MDS=-2.176e-03; 


XRDR=1.148e-03; 


Yvav=-5.643E-02; 
Yvar=2.270E-02; 
Yr2=-4.743E-03; 


Nvav=1.135E-02; 
Nvar=-9.487E-03; 
Nr2=1.607E-03; 


$lat center of rotation off the body 


Yrer]4 Ze 0s. 
Yrav=-3.712E-02; 
Yv2=-5.917E-02; 
Yv3=-3.750e-3; 
Yv4=1.293e-1; 


$end; 


Nrar=-1.770E-03; 

Nrav=1.107E-02; 

Nv2=2.474E-02; 
Nv3=1.132e-2; 
Nv4=-5.463e-2; 
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wavevel.m 


function vel= wavevell(t,x,z,c0,amp,Tw,cx,cy,H) 


%$ Trochoidal wave motion, regular wave train moving in the x-direction 
% v= wave velocity, (ft/sec) 


% amp=wave amplitude, (ft) = wave heigth /2 

% n= wave number, (rad/ft) = w*2/g = 4*Pi*2/Tw*2*g (for gravity 
wave) 

% - Tw= wave period; n = 2*pi/L; with 


Pec grii (a l/ 2/ pay) tanh (2 "pi ay bl /2/ pa 

$ For shallow waters, H<<L: L=T*sqrt(g*H) ; 

U=vehicle speed 

cQ = wave speed 

HEad seas only at this time 

cx= water current flowing in the x-direction (ft/sec) 
cy= water current flowing in the y-direction (ft/sec) 
x= x-global position of the vehicle 

Z= z-global position of the vehicle 

t= time (sec) 


oP OP oO CP CH CH OP cP 


rho=1.94;U0=6; 

g=32.2; L=Tw*sqrt(g*H); 

Ei—Sinhni2 pi* (H-z)/&);sZ—sinh(2*pi* (H/L) ); 
el—cosh (2 "pi * (H-z)'/ Lb); e2—cosh{Z*pi-~(H/L)); 


dx=2*pi*amp/Tw*cl/s2*cos (2*pi* ( (x-U0*t) /L-t/Tw) ) +cx+U0; 


dy=cy; 
dz=2*pi*amp/Tw*sl1/s2*sin(2*pi* ( (x-U0*t) /L-t/Tw) ) ; 
fax, dz] 

dxang= 0; 

dyang= 0; 

dzang= 0; 


pz= xrho*g*amp*cl/c2*cos (2*pi* (x/L-t (1-U0/c0) /Tw) ) ; 


wel—(dx,dy,dz,dxang, dyang, dzang]; 


lll 





APPENDIX C. LIST OF SUMULATION RUNS FOR THE 21UUV MODEL 


This appendix contains a list of all simulation runs for the 21UUV computer 


model. 
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